//----------------------------------------------------------------------------------------------
//	Filename:	main.cpp
//	Author:		Keith Bugeja
//	Date:		27/02/2010
//----------------------------------------------------------------------------------------------
// TODO:
// Double check ILight-derived classes ... some methods have not been tested properly.
// ?? DistributedRenderer should not instantiate MPI - change it to have it passed to the object
// Polish object factories
// Move factories to CorePlugins.dll
// Finish scene loaders
//----------------------------------------------------------------------------------------------
 
//----------------------------------------------------------------------------------------------
//	Set Illumina PRT compilation mode (SHM or DSM)
//----------------------------------------------------------------------------------------------
#include <windows.h>

//----------------------------------------------------------------------------------------------
//	Set Illumina PRT version
//----------------------------------------------------------------------------------------------
namespace Illumina { namespace Core { const int Major = 0; const int Minor = 5; const int Build = 0; } }

using namespace Illumina::Core;

//----------------------------------------------------------------------------------------------
//	Include basic headers for OpenMP and io, string and file streams
//----------------------------------------------------------------------------------------------
#include <omp.h>
#include <iostream>
#include <sstream>
#include <fstream>

//----------------------------------------------------------------------------------------------
//	Include boost header files for managing program options and file paths
//----------------------------------------------------------------------------------------------
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/thread.hpp>
#include <queue>

//----------------------------------------------------------------------------------------------
#include "Logger.h"
#include "Environment.h"
#include "PointCloudSolver.h"
#include "InputHandler.h"

boolean accuReset=false;

//----------------------------------------------------------------------------------------------
class RenderThread 
{
public:
	struct RenderThreadTile
	{
		short XStart, YStart; 
		short XSize, YSize;
	};

	struct RenderThreadStatistics
	{
		double	FirstBarrier,
				JobTime,
				SecondBarrier;

		int		JobCount;
	};

	class RenderThreadState
	{
	protected:
		// Shared tile ID 
		int m_nTileID;
	
		// Run flag
		bool m_bIsRunning;

		// Tile details
		int m_nWidth, m_nHeight;

		// Frame buffer
		RadianceBuffer *m_pRadianceBuffer;

		// Current renderer
		IRenderer *m_pRenderer;

		// Barrier for frame synchronisation
		int m_nThreadCount;

		boost::barrier *m_pRenderBarrier;
		boost::mutex *m_pStatLock;

		// Stat list
		std::vector< std::vector<RenderThread::RenderThreadStatistics>* > m_statistics;
	
		std::vector< RenderThread::RenderThreadTile> m_tilePacket;

	public:
		RenderThreadState(IRenderer *p_pRenderer, RadianceBuffer *p_pRadianceBuffer, int p_nTileWidth, int p_nTileHeight, int p_nThreadCount)
			: m_pRenderer(p_pRenderer)
			, m_pRadianceBuffer(p_pRadianceBuffer)
			, m_nWidth(p_nTileWidth)
			, m_nHeight(p_nTileHeight)
			, m_pRenderBarrier(new boost::barrier(p_nThreadCount))
			, m_pStatLock(new boost::mutex())
			, m_nTileID(0)
			, m_nThreadCount(p_nThreadCount)
		{ }

		~RenderThreadState(void)
		{
			for (std::vector< std::vector<RenderThread::RenderThreadStatistics>* >::iterator iter = m_statistics.begin();
				 iter != m_statistics.end(); iter++) 
			{
				delete *iter;
			}

			m_statistics.clear();
		}

		inline IRenderer* GetRenderer(void) { return m_pRenderer; }
		inline RadianceBuffer* GetRadianceBuffer(void) { return m_pRadianceBuffer; }
		inline int GetThreadCount(void) { return m_nThreadCount; }
		inline int GetWidth(void) { return m_nWidth; }
		inline int GetHeight(void) { return m_nHeight; }
		inline void Reset(void) { m_nTileID = 0; }
		inline void Wait(void) { m_pRenderBarrier->wait(); }
		inline void Run(void) { m_bIsRunning = true; }
		inline bool IsRunning(void) { return m_bIsRunning; }
		inline void Stop(void) { m_bIsRunning = false; }
		inline int NextTile(void) 
		{ 
			return AtomicInt32::FetchAndAdd(&m_nTileID, 1); 
			// return AtomicInt32::Increment(&m_nTileID);
		}
		
		inline void PushThreadStatistics(std::vector<RenderThread::RenderThreadStatistics>* p_pThreadStats)
		{
			m_pStatLock->lock();
			m_statistics.push_back(p_pThreadStats);
			m_pStatLock->unlock();
		}

		std::string GetStatistics(void) 
		{
			std::stringstream message;

			for (int threadID = 0; threadID < m_statistics.size(); threadID++)
			{
				std::vector<RenderThread::RenderThreadStatistics> *pThreadStatList = 
					m_statistics[threadID];

				message << " -- Thread ID : [" << threadID << "]" << std::endl;

				for (int frameNumber = 0; frameNumber < pThreadStatList->size(); frameNumber++)
				{
					message << " --- Frame : [" << frameNumber << "]" << std::endl 
							<< " ---- Barrier 1 : " << (*pThreadStatList)[frameNumber].FirstBarrier << std::endl 
							<< " ---- Computation : " <<  (*pThreadStatList)[frameNumber].JobTime << std::endl
							<< " ---- Barrier 2 : " <<  (*pThreadStatList)[frameNumber].SecondBarrier << std::endl
							<< " ---- Jobs : " <<  (*pThreadStatList)[frameNumber].JobCount << std::endl << std::endl;
				}
			}

			return message.str();
		}

		inline RenderThread::RenderThreadTile &GetTilePacket(int p_nTileID)
		{
			return m_tilePacket[p_nTileID];
		}

		inline int GetTilePacketCount(void)
		{
			return m_tilePacket.size();
		}

		void GenerateTilePackets(int p_nStepSize)
		{
			int tileSize = Maths::Max(m_nWidth, m_nHeight),
				varTileSize = tileSize;

			int tilesPerRow = m_pRadianceBuffer->GetWidth() / tileSize,
				tilesPerCol = m_pRadianceBuffer->GetHeight() / tileSize,
				tilesPerPage = tilesPerRow * tilesPerCol;

			int tileX, tileY, 
				stepIncrement = 0, 
				currentStep = 1;

			RenderThread::RenderThreadTile tilePacket;

			for (int tileID = 0; tileID < tilesPerPage; tileID++)
			{
				tileX = (tileID % tilesPerRow) * tileSize,
				tileY = (tileID / tilesPerRow) * tileSize;

				tilePacket.XSize = varTileSize;
				tilePacket.YSize = varTileSize;

				for (int subTileY = 0; subTileY < currentStep; subTileY++)
				{
					for (int subTileX = 0; subTileX < currentStep; subTileX++)
					{
						tilePacket.XStart = tileX + subTileX * varTileSize;
						tilePacket.YStart = tileY + subTileY * varTileSize;
							
						m_tilePacket.push_back(tilePacket);
					}
				}
				
				if (varTileSize <= 16)
					continue;

				if (++stepIncrement == p_nStepSize)
				{
					stepIncrement = 0;
					currentStep <<= 1;
					varTileSize >>= 1;
				}
			}
		}
	};

public:

	std::queue<int> buff;

	// Thread body
	static void Render(RenderThreadState *p_pState)
	{
		IRenderer *pRenderer = p_pState->GetRenderer();
		RadianceBuffer *pRadianceBuffer = p_pState->GetRadianceBuffer();		

		int tileID, tilesPerPage = p_pState->GetTilePacketCount();

		// Add statistics log and push onto state
		std::vector<RenderThread::RenderThreadStatistics>* pThreadStatList 
			= new std::vector<RenderThread::RenderThreadStatistics>();
		p_pState->PushThreadStatistics(pThreadStatList);

		// Use as container for each frame
		RenderThread::RenderThreadStatistics threadStats;
		
		while (p_pState->IsRunning())
		{
			// Reset job count for frame
			threadStats.JobCount = 0;

			// Time first barrier
			threadStats.FirstBarrier = Platform::GetTime();
			p_pState->Wait();
			threadStats.FirstBarrier = Platform::ToSeconds(Platform::GetTime() - threadStats.FirstBarrier);

			// Time job
			threadStats.JobTime = Platform::GetTime();
			while((tileID = p_pState->NextTile()) < tilesPerPage)			
			{
				// std::cout << boost::this_thread::get_id() << " : " << tileID << std::endl;
				const RenderThread::RenderThreadTile &tilePacket = p_pState->GetTilePacket(tileID);
				
				/**/
				pRenderer->RenderRegion(pRadianceBuffer, tilePacket.XStart, tilePacket.YStart, tilePacket.XSize, tilePacket.YSize, tilePacket.XStart, tilePacket.YStart);
				/**/

				/*
				pRenderer->RenderRegion(pRadianceBuffer, tilePacket.XStart + 1, tilePacket.YStart + 1, 
					tilePacket.XSize - 1, tilePacket.YSize - 1, tilePacket.XStart + 1, tilePacket.YStart + 1);
				/**/

				// Increment jobs done
				threadStats.JobCount++;
			}
			threadStats.JobTime = Platform::ToSeconds(Platform::GetTime() - threadStats.JobTime);

			// Time second barrier
			threadStats.SecondBarrier = Platform::GetTime();
			p_pState->Wait();
			threadStats.SecondBarrier = Platform::ToSeconds(Platform::GetTime() - threadStats.SecondBarrier);

			// Save frame stats
			pThreadStatList->push_back(threadStats);
		}
	}
};

struct MouseStates
{ 
	int m_leftState;
	int m_rightState;
	int m_lastLeft;
	int m_lastRight;
 
	char m_mouse[2];
 
	void UpdateMouse()
	{
		//set old states
		m_lastLeft = m_leftState;
		m_lastRight = m_rightState;
		//Find out if the button is down
		m_mouse[0] = (char) (GetAsyncKeyState(1) >> 8);
		m_mouse[1] = (char) (GetAsyncKeyState(2) >> 8);
 
		if (m_mouse[0] != 0) m_leftState = 1;
		else m_leftState = 0;
 
		if (m_mouse[1] != 0) m_rightState = 1;
		else m_rightState = 0;
	}
 
	int GetButtonState(int button)
	{
		switch(button)
		{
		case 1:
 
			//Check if it's up
			if ( m_leftState == 0 && m_leftState == m_lastLeft ) return 0;
			//Check if it's just gone down
			if ( m_leftState == 1 && m_leftState != m_lastLeft ) return 1;
			//Check if it's been released
			if ( m_leftState == 0 && m_leftState != m_lastLeft ) return 2;
			//Check if it's held
			if ( m_leftState == 1 && m_leftState == m_lastLeft ) return 3;
 
			break;
		case 2:
 
			//Check if it's up
			if ( m_rightState == 0 && m_rightState == m_lastRight ) return 0;
			//Check if it's just gone down
			if ( m_rightState == 1 && m_rightState != m_lastRight ) return 1;
			//Check if it's been released
			if ( m_rightState == 0 && m_rightState != m_lastRight ) return 2;
			//Check if it's held
			if ( m_rightState == 1 && m_rightState == m_lastRight ) return 3;
			break;
 
		default:
			return -1;
		}
	}
 
};

struct KeyboardStates
{  
	enum key_state { clicked, pressed, released, up, invalid };
	bool m_keyboard_state[256];
 
	KeyboardStates::KeyboardStates()
	{
		for (int i = 0; i<256; i++) m_keyboard_state[i] = false;
	}

	key_state GetKeyState(int p_key)
	{		
		if (p_key < 256)
		{
			bool pressed = (GetAsyncKeyState(p_key) & 0x8000);

			//std::cout << "Key = [" << p_key << "], State = [" << m_keyboard_state[p_key] << "], New = [" << pressed << "]" << std::endl;

			if (m_keyboard_state[p_key])
			{
				m_keyboard_state[p_key] = pressed;

				return (pressed) ? KeyboardStates::pressed
					: KeyboardStates::released;
			}
			else
			{
				m_keyboard_state[p_key] = pressed;

				return (pressed) ? KeyboardStates::clicked
					: KeyboardStates::up;
			}
		}

		return KeyboardStates::invalid;		
	}
 
};

void SaveCameraParameters(Environment *p_environment, std::map<std::string, std::pair<std::vector<Vector3>, std::vector<float>>> * p_cameraParameters)
{
	std::string camera_parameters_location = p_environment->GetCameraParametersStore();	
	int saved_count = 0;

	//Read file 
	FILE *fp;
	bool quit = false;
	if (!(fp = fopen(camera_parameters_location.c_str(), "w")))
	{
		std::cout << "[CameraParameters] - Unable to open camera parameters file " << camera_parameters_location << std::endl;
		quit = true;
	}
	else
	{		
		std::cout << "[CameraParameters] - Storing camera parameters file " << camera_parameters_location << std::endl;
		std::map<std::string, std::pair<std::vector<Vector3>, std::vector<float>>>::iterator cpIterator;
		std::string scene_name;
		Vector3 cam_pos, cam_target, cam_up;
		float rot_x, rot_y, rot_z;
		for (cpIterator = p_cameraParameters->begin(); cpIterator != p_cameraParameters->end(); cpIterator++)
		{
			scene_name = cpIterator->first;
			cam_pos		= cpIterator->second.first[0];
			cam_target	= cpIterator->second.first[1];
			cam_up		= cpIterator->second.first[2];

			rot_x = cpIterator->second.second[0];
			rot_y = cpIterator->second.second[1];
			rot_z = cpIterator->second.second[2];

			fputs (scene_name.c_str(),fp);
			fprintf(fp, " %f %f %f %f %f %f %f %f %f %f %f %f\n", cam_pos.X, cam_pos.Y, cam_pos.Z, cam_target.X, cam_target.Y, cam_target.Z, cam_up.X, cam_up.Y, cam_up.Z, rot_x, rot_y, rot_z);
			saved_count++;
		}
		fclose(fp);
	}
	std::cout << "[CameraParametersLoader] Stored " << saved_count << " camera parameters" << std::endl;
}

//Load Camera Parameters (optimal for viewing) of previously viewed point clouds
std::map<std::string, std::pair<std::vector<Vector3>, std::vector<float>>> * LoadCameraParameters(Environment *p_environment)
{	
	std::string camera_parameters_location = p_environment->GetCameraParametersStore();
	std::map<std::string, std::pair<std::vector<Vector3>, std::vector<float>>> * camera_parameters = new std::map<std::string, std::pair<std::vector<Vector3>, std::vector<float>>>();

	typedef boost::tokenizer<boost::char_separator<char>> tokenizer;	
	boost::char_separator<char> sep(" \n");
	tokenizer::iterator beg;		

	int loaded_count = 0;

	//Read file 
	FILE *fp;
	bool quit = false;
	if (!(fp = fopen(camera_parameters_location.c_str(), "r")))
	{
		std::cout << "[Training Phase] - Unable to open camera parameters file " << camera_parameters_location << std::endl;
		quit = true;
	}
	else
	{
		std::cout << "[Training Phase] - Reading from camera parameters file " << camera_parameters_location << std::endl;
		char buffer[512];
		int line_count = 0;
		std::string model_name, line_data;
		float cam_position_x, cam_position_y, cam_position_z;
		float cam_target_x, cam_target_y, cam_target_z;			
		float cam_up_x, cam_up_y, cam_up_z;
		float theta_x, theta_y, theta_z;

		while (fgets(buffer, 512, fp))
		{
				line_count++;
				char *bufferp = buffer;
				while (isspace(*bufferp)) bufferp++;
				
				if (*bufferp == '#') continue;   //skip comments
				if (*bufferp == '\0') continue;	 //and blank lines				
				
				line_data = buffer;
				tokenizer tok(line_data, sep);
				beg = tok.begin();
				model_name = *beg; ++beg;

				cam_position_x = boost::lexical_cast<float, std::string>(*beg); ++beg;
				cam_position_y = boost::lexical_cast<float, std::string>(*beg); ++beg;
				cam_position_z = boost::lexical_cast<float, std::string>(*beg); ++beg;

				cam_target_x = boost::lexical_cast<float, std::string>(*beg); ++beg;
				cam_target_y = boost::lexical_cast<float, std::string>(*beg); ++beg;
				cam_target_z = boost::lexical_cast<float, std::string>(*beg); ++beg;

				cam_up_x = boost::lexical_cast<float, std::string>(*beg); ++beg;
				cam_up_y = boost::lexical_cast<float, std::string>(*beg); ++beg;
				cam_up_z = boost::lexical_cast<float, std::string>(*beg); ++beg;

				theta_x = boost::lexical_cast<float, std::string>(*beg); ++beg;
				theta_y = boost::lexical_cast<float, std::string>(*beg); ++beg;
				theta_z = boost::lexical_cast<float, std::string>(*beg);
				
				(*camera_parameters)[model_name] = std::pair<std::vector<Vector3>, std::vector<float>>();
				(*camera_parameters)[model_name].first.push_back(Vector3(cam_position_x, cam_position_y, cam_position_z));
				(*camera_parameters)[model_name].first.push_back(Vector3(cam_target_x, cam_target_y, cam_target_z));
				(*camera_parameters)[model_name].first.push_back(Vector3(cam_up_x, cam_up_y, cam_up_z));
				(*camera_parameters)[model_name].second.push_back(theta_x);
				(*camera_parameters)[model_name].second.push_back(theta_y);
				(*camera_parameters)[model_name].second.push_back(theta_z);

				loaded_count++;
			}
			fclose(fp);
		}
		std::cout << "[CameraParametersLoader] Loaded " << loaded_count << " camera parameters" << std::endl;		
		return camera_parameters;
}

void ReadFromConsole(Environment *pEnvironment, EngineKernel *pKernel, PointCloudSolver *pcs, float p_fCameraSpeed) 
{
	std::cout << "[Input Handling] Press enter to start registration of mouse and keyboard events" << std::endl;
	std::getchar();

	//MouseStates mouse;
	KeyboardStates key;

	bool restore = false;

	DWORD cNumRead, fdwMode, i; 
	INPUT_RECORD irInBuf[10];
	HANDLE hStdin = GetStdHandle(STD_INPUT_HANDLE);
	HWND hStdinVLC = FindWindow(NULL, "VLC (Direct3D output)");
	if (hStdin == INVALID_HANDLE_VALUE) std::cout << "[Input Handling] Problems with handle value" << std::endl;	
	if (hStdinVLC == NULL) std::cout << "[Input Handling] NULL VLC Window handle value" << std::endl;
	std::cout << "[Input Handling] LAST ERROR=" << std::hex << GetLastError() << std::dec << std::endl;	

	bool quit = false;

	ICamera *pCamera = pEnvironment->GetCamera();

	RECT vlcRect;	

	AccumulationBuffer *pBuffer = (AccumulationBuffer*) pKernel->GetPostProcessManager()->RequestInstance("AccumulationBuffer");

	float theta_y = 0.f;
	float theta_x = 0.f;
	float theta_z = 0.f;

	float window_width = 0;
	float window_height = 0;

	std::vector<std::vector<int>> * sorted_segs;
	int current_segment_view_idx = 0;
	
	boolean processing_visibility_request = false;
	boolean processing_visibility_segmentid = false;
	boolean processing_visibility_request_delay = false;
	std::string visibility_segment_number;
	
	boolean processing_visibility_obbsegmentid = false;
	boolean processing_visibility_obbrequest_delay = false;

	boolean processing_graph_pruning_request = false;
	boolean processing_graph_pruning_request_delay = false;
	std::string graph_pruning_state_number;

	boolean processing_search_request = false;
	boolean processing_search_request_delay = false;
	std::string search_shape_id;

	boolean processing_pointcloud_switch_request = false;	
	boolean processing_pointcloud_switch_request_view = false;
	std::string new_pointcloud_id;
	std::string new_pointcloud_view;
	std::string old_pointcloud_id;
	std::string old_pointcloud_view;

	bool obbs_visible = false;
	bool grids_visible = false;
	bool models_visible = false;

	std::vector<int> b_nodes;

	float grid_rot = 0;
	float grid_scale = 1;
	bool all_points_in_grid = false;

	float u_min_t = 0;
	float u_max_t = 0;
	float v_min_t = 0;
	float v_max_t = 0;
	float w_min_t = 0;
	float w_max_t = 0;

	int nextGridViewId = 0;

	int currentlyViewedSegment = 0;
	int currentlyViewedModelVG = 0;

	bool history_viewing_enabled = false;

	GeometricPrimitive * gp = (GeometricPrimitive*) pEnvironment->GetSpace()->PrimitiveList[0];
	bool scene_change = false;
	Ray ray;

	std::map<std::string, std::pair<std::vector<Vector3>, std::vector<float>>> * camera_parameters = LoadCameraParameters(pEnvironment);
	//------------------------
	//Position camera to pre-saved position if one is availalbe
	std::string trunk = boost::lexical_cast<std::string, int>(((KDTreePointCloud*)(gp->GetShape()))->GetPCSId());	
	if (camera_parameters->find(trunk) != camera_parameters->end())
	{
		std::cout << "[Setting Camera Parameters] - Trunk " << trunk << " found. Resetting Camera Position" << std::endl;
		std::pair<std::vector<Vector3>, std::vector<float>> cam_parameters = camera_parameters->find(trunk)->second;
		pCamera->MoveTo(cam_parameters.first[0]);
		pCamera->Look(cam_parameters.first[1], cam_parameters.first[2]);
		theta_x = cam_parameters.second[0];
		theta_y = cam_parameters.second[1];
		theta_z = cam_parameters.second[2];
		gp->WorldTransform.SetRotation(Matrix3x3::CreateRotation(Vector3::UnitXPos, theta_x) * 
										Matrix3x3::CreateRotation(Vector3::UnitYPos, theta_y) * 
										Matrix3x3::CreateRotation(Vector3::UnitZPos, theta_z));		
		accuReset=true;
	}
	//------------------------


	while (!quit)
	{		
		POINT p;		
		/*mouse.UpdateMouse();
		if (mouse.GetButtonState(1) == 1)
		{			
			if (GetCursorPos(&p))
			{				
				ScreenToClient(hStdinVLC, &p);
				GetWindowRect(hStdinVLC, &vlcRect);
				window_width = vlcRect.right - vlcRect.left;
				window_height = vlcRect.bottom - vlcRect.top;
				pCamera->GetRay(1-(p.x / window_width), 1-(p.y / window_height), 0.f, 0.f, ray);
				std::cout << "WinW=" << window_width << " WinH=" << window_height << " CurPos=" << p.x / window_width << " -- " << p.y / window_height << " Ray=" << ray.Direction.ToString() << std::endl;
				Intersection inter;
				if (pEnvironment->GetScene()->Intersects(ray, inter))
				{							
					accuReset = true;
					//((KDTreePointCloud*)(gp->GetShape()))->SetPointInvisible(inter.Surface.GeometryFragment.GetDataEx());
					((KDTreePointCloud*)(gp->GetShape()))->SetPointNeighbourhoodVisible(inter.Surface.GeometryFragment.GetDataEx());
					std::cout << "Intersection at " << inter.Surface.GeometryFragment.GetDataEx() << std::endl;
				} 
				else 
				{
					//std::cout << "NO Intersection!!!!" << std::endl;
				}
			}
		}

		if (mouse.GetButtonState(2) == 1)
		{			
			if (GetCursorPos(&p))
			{
				//std::cout << "Cursor Pos=" << p.x << " -- " << p.y << std::endl;
				ScreenToClient(hStdinVLC, &p);
				std::cout << "ScreenToClient=" << p.x << " -- " << p.y << std::endl;
			}
		}
		*/
		if (!processing_visibility_request && !processing_pointcloud_switch_request)
		{						
			scene_change = false;
			//Camera Movements
			if (key.GetKeyState('W') == KeyboardStates::clicked) { pCamera->Move(pCamera->GetFrame().GetW() * p_fCameraSpeed); scene_change = true; }
			if (key.GetKeyState('S') == KeyboardStates::clicked) { pCamera->Move(pCamera->GetFrame().GetW() * -p_fCameraSpeed); scene_change = true; }
			if (key.GetKeyState('A') == KeyboardStates::clicked) { pCamera->Move(pCamera->GetFrame().GetU() * p_fCameraSpeed); scene_change = true; }
			if (key.GetKeyState('D') == KeyboardStates::clicked) { pCamera->Move(pCamera->GetFrame().GetU() * -p_fCameraSpeed); scene_change = true; }					 
			if (key.GetKeyState('Q') == KeyboardStates::clicked) { pCamera->Move(pCamera->GetFrame().GetV() * p_fCameraSpeed); scene_change = true; }					
			if (key.GetKeyState('E') == KeyboardStates::clicked) { pCamera->Move(pCamera->GetFrame().GetV() * -p_fCameraSpeed); scene_change = true; }

			//Splat size adjustment
			if (key.GetKeyState(VK_ADD) == KeyboardStates::clicked) { ((KDTreePointCloud*)(gp->GetShape()))->SetRadius(((KDTreePointCloud*)(gp->GetShape()))->GetRadius() + 0.01); scene_change = true; }
			if (key.GetKeyState(VK_SUBTRACT) == KeyboardStates::clicked) { ((KDTreePointCloud*)(gp->GetShape()))->SetRadius(((KDTreePointCloud*)(gp->GetShape()))->GetRadius() - 0.01); scene_change = true; }

			//PointCloud Movements
			if (key.GetKeyState(VK_NUMPAD8) == KeyboardStates::clicked) { theta_x-=0.1f; scene_change = true; }					
			if (key.GetKeyState(VK_NUMPAD2) == KeyboardStates::clicked) { theta_x+=0.1f; scene_change = true; }						
			if (key.GetKeyState(VK_NUMPAD4) == KeyboardStates::clicked) { theta_y-=0.1f; scene_change = true; }						
			if (key.GetKeyState(VK_NUMPAD6) == KeyboardStates::clicked) { theta_y+=0.1f; scene_change = true; }						
			if (key.GetKeyState(VK_NUMPAD7) == KeyboardStates::clicked) { theta_z-=0.1f; scene_change = true; }					
			if (key.GetKeyState(VK_NUMPAD9) == KeyboardStates::clicked) { theta_z+=0.1f; scene_change = true; }

			//Print|Update Camera Parameters
			if (key.GetKeyState(VK_F9) == KeyboardStates::clicked) { std::cout << "[Current Camera Parameters] " << pCamera->ToString() << std::endl; }
			
			if (key.GetKeyState(VK_F10) == KeyboardStates::clicked) 
			{ 
				//std::string trunk = ((KDTreePointCloud*)(gp->GetShape()))->GetTrunk();				
				//if (camera_parameters->find(trunk) != camera_parameters->end())
				std::string trunk = boost::lexical_cast<std::string, int>(((KDTreePointCloud*)(gp->GetShape()))->GetPCSId());
				if (camera_parameters->find(trunk) != camera_parameters->end())
				{
					std::cout << "[Setting Camera Parameters] - Trunk " << trunk << " found. Resetting Camera Position" << std::endl;
					std::pair<std::vector<Vector3>, std::vector<float>> cam_parameters = camera_parameters->find(trunk)->second;
					pCamera->MoveTo(cam_parameters.first[0]);
					pCamera->Look(cam_parameters.first[1], cam_parameters.first[2]);
					theta_x = cam_parameters.second[0];
					theta_y = cam_parameters.second[1];
					theta_z = cam_parameters.second[2];
					scene_change = true;
				}
				else
				{
					std::cout << "[Setting Camera Parameters] - Trunk " << trunk << " not found in previously stored parameters" << std::endl; 
				}
			}			

			if (key.GetKeyState(VK_F11) == KeyboardStates::clicked)
			{
				std::string trunk = boost::lexical_cast<std::string, int>(((KDTreePointCloud*)(gp->GetShape()))->GetPCSId());				
				std::cout << "[Camera Parameters] Setting Parameters for " << trunk << std::endl;
				(*camera_parameters)[trunk] = std::pair<std::vector<Vector3>, std::vector<float>>();
				(*camera_parameters)[trunk].first.clear();
				(*camera_parameters)[trunk].second.clear();
				(*camera_parameters)[trunk].first.push_back(pCamera->GetObserver());
				(*camera_parameters)[trunk].first.push_back(pCamera->GetFrame().GetW());
				(*camera_parameters)[trunk].first.push_back(pCamera->GetFrame().GetV());				
				(*camera_parameters)[trunk].second.push_back(theta_x);
				(*camera_parameters)[trunk].second.push_back(theta_y);
				(*camera_parameters)[trunk].second.push_back(theta_z);
				scene_change = true;
			}

			if (key.GetKeyState(VK_F12) == KeyboardStates::clicked)
			{
				std::cout << "[Camera Parameters] Persisting Camera Parameters #count = " << camera_parameters->size() << std::endl;
				SaveCameraParameters(pEnvironment, camera_parameters);
			}

			//-----
			if (scene_change) 
			{
				accuReset = true;
				gp->WorldTransform.SetRotation(Matrix3x3::CreateRotation(Vector3::UnitXPos, theta_x) *
												Matrix3x3::CreateRotation(Vector3::UnitYPos, theta_y) *
												Matrix3x3::CreateRotation(Vector3::UnitZPos, theta_z));
				((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->UpdateWorldTransforms(gp->WorldTransform);
				scene_change = false;
			}
		}		

		//**********************************************************
		//Visibility Settings Processing
		if (!processing_visibility_request && !processing_pointcloud_switch_request && !processing_search_request && key.GetKeyState('V') == KeyboardStates::clicked)
		{
			processing_visibility_request = true;
			std::cout << "[Main] Entering Visibility Request Mode ... " << std::endl;
			processing_visibility_request_delay = true;
		}

		//Markov Search Settings Processing Flagged
		if (!processing_visibility_request && !processing_pointcloud_switch_request && !processing_search_request && key.GetKeyState('M') == KeyboardStates::clicked)
		{
			processing_search_request = true;
			std::cout << "[Main] Entering Search Request Mode ... " << std::endl;
			processing_search_request_delay = true;
		}		

		//GridView Processing Flagged
		if (!processing_visibility_request && !processing_pointcloud_switch_request && !processing_search_request && key.GetKeyState('G') == KeyboardStates::clicked)
		{
			//processing_grid_request = true;
			std::cout << "[Main] Viewing VoxelGrid of Currently Set Segment: " << visibility_segment_number << std::endl;
			
			GeometricPrimitive * gp = new GeometricPrimitive();			
			PointCloudSegmentation * model_pcs = pcs->m_trainedModelsDB->GetByName("chairs", new_pointcloud_id);
			if (model_pcs == NULL) std::cout << "MODEL IS NULL ... " << new_pointcloud_id << std::endl;	
			else { 
				std::cout << "MODEL LOADED ... "; model_pcs->ToString(); 
				FeatureDescStructureGraph * fdg = model_pcs->GetFDG(boost::lexical_cast<int, std::string>(visibility_segment_number));					

				((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->ClearAllGrids();

				std::vector<std::pair<std::pair<int, float>, Vector3>> * grid_spheres;
				if (nextGridViewId==0) grid_spheres = fdg->m_grid_u_v_w->GetActiveCells_GridView();				 
				if (nextGridViewId==1) grid_spheres = fdg->m_grid_u_v_inv_w->GetActiveCells_GridView();
				if (nextGridViewId==2) grid_spheres = fdg->m_grid_u_v_w_inv->GetActiveCells_GridView();
				if (nextGridViewId==3) grid_spheres = fdg->m_grid_u_v_inv_w_inv->GetActiveCells_GridView();
				if (nextGridViewId==4) grid_spheres = fdg->m_grid_u_inv_v_w->GetActiveCells_GridView();
				if (nextGridViewId==5) grid_spheres = fdg->m_grid_u_inv_v_inv_w->GetActiveCells_GridView();
				if (nextGridViewId==6) grid_spheres = fdg->m_grid_u_inv_v_w_inv->GetActiveCells_GridView();
				if (nextGridViewId==7) grid_spheres = fdg->m_grid_u_inv_v_inv_w_inv->GetActiveCells_GridView();

				nextGridViewId = (nextGridViewId+1)%8;
				std::cout << "Number of spheres = " << grid_spheres->size() << std::endl;
				KDTreeSphereMesh * sphereMesh = new KDTreeSphereMesh(new_pointcloud_id+"_grid");
				int g = 0; 
				for (auto s : *grid_spheres) 
				{ 
					if (s.first.first) g = 0; else g = 1; 
					sphereMesh->AddSphere(s.second, s.first.second, g); 
				}
				sphereMesh->ComputeBoundingVolume();
				std::cout << "SphereMesh Bounding Volume = " << sphereMesh->GetBoundingVolume()->ToString() << std::endl;
				sphereMesh->Compile();
				gp->SetShape(sphereMesh);
				gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;			
				gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("GridMaterial"));
				((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddSegmentGrid(new_pointcloud_id+":"+visibility_segment_number, gp);
				scene_change=true;
				accuReset = true;
			}
		}
		
		struct vg_stats {
			FeatureDescStructureGraph::VoxelGrid * vgrid;
			FeatureDescStructureGraph::VoxelGrid * tgrid;
			Vector3 best_min_uvw_t;
			Vector3 best_max_uvw_t;
			float best_rot;
			int min_diff;
			int max_pos;
			int max_neg;				
			int total_vg_built;			
			float max_coverage;
			float coverage;
			int points_out;

			vg_stats(FeatureDescStructureGraph::VoxelGrid * _tgrid)
				: best_rot(0)
				, vgrid(new FeatureDescStructureGraph::VoxelGrid(1,1,1))
				, tgrid(_tgrid)
				, best_min_uvw_t(0)
				, best_max_uvw_t(0)
				, min_diff(INT_MAX)
				, max_pos(0)
				, max_neg(0)
				, total_vg_built(0)
				, max_coverage(0)
				, coverage(0)
				, points_out(0)
			{}									
		};

		//GridView Processing On Scene Segment
		if (!processing_visibility_request && !processing_pointcloud_switch_request && !processing_search_request && key.GetKeyState('K') == KeyboardStates::clicked)
		{
			//processing_grid_request = true;
			//std::cout << "[Main] Viewing VoxelGrid of Currently Set Segment: " << visibility_segment_number << std::endl;
			((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->ClearAllGrids();

			GeometricPrimitive * gp = new GeometricPrimitive();			
			PointCloudSegmentation * model_pcs = pcs->m_trainedModelsDB->GetByName("chairs", "10000");
			if (model_pcs == NULL) std::cout << "MODEL IS NULL ... " << new_pointcloud_id << std::endl;
			else {
				//std::cout << "MODEL LOADED ... " << model_pcs->ToString() << std::endl;
				FeatureDescStructureGraph * fdg = model_pcs->GetFDG(12);
				int rotationCount = 24;
				float rot_diff = Maths::PiTwo / rotationCount;
				double start = Platform::GetTime();
				float scale_factor = 0.15f;
				std::pair<float, std::pair<float, float>> voxel_grid_measure;

				Vector3 best_obb_scale;
				Vector3 best_min_uvw_t;
				Vector3 best_max_uvw_t;
				float best_rot;
				int min_diff = 1000000;
				int max_pos = 0;
				int max_neg = 0;
				int total_vg_built = 0; 
				float t_step = 0.6f;
				float max_coverage = 0;
				float coverage = 0;
				int best_grid = -1;					
				
				std::vector<vg_stats> model_grids;
				model_grids.push_back(vg_stats(fdg->m_grid_u_v_w));
				model_grids.push_back(vg_stats(fdg->m_grid_u_v_inv_w));
				model_grids.push_back(vg_stats(fdg->m_grid_u_v_w_inv));
				model_grids.push_back(vg_stats(fdg->m_grid_u_v_inv_w_inv));
				model_grids.push_back(vg_stats(fdg->m_grid_u_inv_v_w));
				model_grids.push_back(vg_stats(fdg->m_grid_u_inv_v_inv_w));
				model_grids.push_back(vg_stats(fdg->m_grid_u_inv_v_w_inv));
				model_grids.push_back(vg_stats(fdg->m_grid_u_inv_v_inv_w_inv));

				int average_grid_size = (fdg->m_grid_u_v_w->m_voxels->size() + 
								fdg->m_grid_u_v_inv_w->m_voxels->size() +
								fdg->m_grid_u_v_w_inv->m_voxels->size() +
								fdg->m_grid_u_v_inv_w_inv->m_voxels->size() +
								fdg->m_grid_u_inv_v_w->m_voxels->size() +
								fdg->m_grid_u_inv_v_inv_w->m_voxels->size() +
								fdg->m_grid_u_v_w_inv->m_voxels->size() +
								fdg->m_grid_u_inv_v_inv_w_inv->m_voxels->size()) / 8;
				
				FeatureDescStructureGraph::VoxelGrid * crt_model_grid;
				FeatureDescStructureGraph::VoxelGrid * voxel_grid = new FeatureDescStructureGraph::VoxelGrid(1, 1, 1);
				
				//std::vector<int> segs; segs.push_back(374); segs.push_back(375);
				std::vector<int> segs; segs.push_back(542); segs.push_back(543); //segs.push_back(374); //segs.push_back(129);
				//std::vector<int> segs; segs.push_back(369); segs.push_back(370); segs.push_back(371);
				//std::vector<int> segs; segs.push_back(378); segs.push_back(379); segs.push_back(380);
				
				int total_points_in_segs = 0;
				for (auto s : segs)
				{
					total_points_in_segs += pcs->m_sceneToSolve->m_structureGraph->m_nodes->at(s)->segment->points->size();
				}

				#pragma omp parallel for
				for (int vg_idx = 0; vg_idx<model_grids.size(); vg_idx++)
				{
					vg_stats * crt_stats = &(model_grids.at(vg_idx));
					FeatureDescStructureGraph::VoxelGrid * crt_model_grid = crt_stats->tgrid;
					bool all_points_in_grid = false;
					Vector3 min_t(0); Vector3 max_t(0);
					for (int i = 0; i<rotationCount; i++)
					{
						all_points_in_grid = false;
						float inclusion_bias = 0.f;
						min_t.X = 0; min_t.Y = 0; min_t.Z = 0;
						max_t.X = 0; max_t.Y = 0; max_t.Z = 0;
						while (!all_points_in_grid)
						{							
							pcs->m_sceneToSolve->ComputeVoxelGridAroundSegment(segs, crt_model_grid, 1, rot_diff*i, min_t, max_t, crt_stats->vgrid);
							crt_stats->total_vg_built++;						
							//crt_stats->coverage = crt_model_grid->ComputeOverlapPosWithGridUVW(crt_stats->vgrid).second / (float)average_grid_size;
							int points_out = crt_stats->vgrid->above_u_max + crt_stats->vgrid->below_u_min +
											crt_stats->vgrid->below_v_min + crt_stats->vgrid->above_v_max +
											crt_stats->vgrid->below_w_min + crt_stats->vgrid->above_w_max;
							if (segs.size() == 1) inclusion_bias = 0.2f; 
							else 
							{
								points_out == 0 ? inclusion_bias = 0.f : inclusion_bias = ((float)points_out / (float)total_points_in_segs) / 5.0f;
							}

							if (crt_stats->coverage - (inclusion_bias) >= crt_stats->max_coverage)
							{								
								crt_stats->max_coverage = crt_stats->coverage - (inclusion_bias);
								crt_stats->points_out = points_out;
								//std::cout << "Inclusion Bias Set to " << inclusion_bias << " Max_Coverage to " << crt_stats->max_coverage << std::endl;
								//voxel_grid_measure = crt_model_grid->ComputeOverlapDiffWithGridUVW(voxel_grid);
								//max_pos = voxel_grid_measure.second.first;
								//max_neg = voxel_grid_measure.second.second;
								//min_diff = voxel_grid_measure.first;	
								crt_stats->best_rot = rot_diff*i;
								crt_stats->best_min_uvw_t.X = min_t.X; crt_stats->best_min_uvw_t.Y = min_t.Y; crt_stats->best_min_uvw_t.Z = min_t.Z;
								crt_stats->best_max_uvw_t.X = max_t.X; crt_stats->best_max_uvw_t.Y = max_t.Y; crt_stats->best_max_uvw_t.Z = max_t.Z; 																							
							}	

							all_points_in_grid=true;
							if (crt_stats->vgrid->above_u_max > 0 | crt_stats->vgrid->below_u_min > 0) { min_t.X += t_step; max_t.X += t_step; all_points_in_grid = false; } // std::cout << " U++ Above::" << voxel_grid->above_u_max << " Below::" << voxel_grid->below_u_min << std::endl; }
							if (crt_stats->vgrid->below_v_min > 0) { min_t.Y += t_step; all_points_in_grid=false; } // std::cout << " BelowV::" << voxel_grid->below_v_min << std::endl; }
							if (crt_stats->vgrid->above_v_max > 0) { max_t.Y += t_step; all_points_in_grid=false; } // std::cout << " AboveV::" << voxel_grid->above_v_max << std::endl; }
							if (crt_stats->vgrid->below_w_min > 0) { min_t.Z += t_step; all_points_in_grid=false; } // std::cout << " BelowW::" << voxel_grid->below_w_min << std::endl; }
							if (crt_stats->vgrid->above_w_max > 0) { max_t.Z += t_step; all_points_in_grid=false; } // std::cout << " AboveW::" << voxel_grid->above_w_max << std::endl; }
						}											
						min_t.X = 0; min_t.Y = 0; min_t.Z = 0;
						max_t.X = 0; max_t.Y = 0; max_t.Z = 0;
					}
				}				
				
				double end = Platform::GetTime();
				
				std::cout << "Time Elapsed::" << Platform::ToSeconds(end-start) << " ... AND THE RESULTS ARE ****** " << std::endl;

				for (int i=0; i<model_grids.size(); i++)
				{
					std::cout << "Grid:" << i << " Max Coverage::" << model_grids.at(i).max_coverage << " Points Out::" << model_grids.at(i).points_out << " Model VG Size::" <<  model_grids.at(i).tgrid->m_voxels->size() << std::endl;				
					if (model_grids.at(i).max_coverage > max_coverage) { max_coverage = model_grids.at(i).max_coverage; best_grid = i; }
					total_vg_built += model_grids.at(i).total_vg_built;
				}
				
				//best_grid = 1;

				crt_model_grid = model_grids.at(best_grid).tgrid;			
				voxel_grid = pcs->m_sceneToSolve->ComputeVoxelGridAroundSegment(segs, crt_model_grid, 1, model_grids.at(best_grid).best_rot, model_grids.at(best_grid).best_min_uvw_t, model_grids.at(best_grid).best_max_uvw_t);
				voxel_grid_measure = crt_model_grid->ComputeOverlapDiffWithGridUVW(voxel_grid);
				model_grids.at(best_grid).max_pos	= voxel_grid_measure.second.first;
				model_grids.at(best_grid).max_neg	= voxel_grid_measure.second.second;
				model_grids.at(best_grid).min_diff	= voxel_grid_measure.first;
				std::cout << best_grid << ") Total VGs Computed::" << total_vg_built << " MaxCoverage::" << model_grids.at(best_grid).max_coverage << " +Max::" << model_grids.at(best_grid).max_pos << " -Max::" << model_grids.at(best_grid).max_neg 
					<< " DiffMin::" << model_grids.at(best_grid).min_diff << " BestRot::" << model_grids.at(best_grid).best_rot << " BestMinUVWT::" << model_grids.at(best_grid).best_min_uvw_t.ToString() << " BestMaxUVWT::" << 
					model_grids.at(best_grid).best_max_uvw_t.ToString() << std::endl;
				std::cout << model_grids.at(best_grid).vgrid->ToString();				
				

				/*
				//Use this code to view voxel grids rendered directly in the current scene 
				//FeatureDescStructureGraph::VoxelGrid * voxel_grid;				
				currentlyViewedModelVG = 6;
				crt_model_grid = model_grids.at(currentlyViewedModelVG).tgrid;
				std::cout << "Using grid ID::" << currentlyViewedModelVG << std::endl;
				//std::vector<int> segs; segs.push_back(boost::lexical_cast<int, std::string>(visibility_segment_number));					
							
				pcs->m_sceneToSolve->ComputeVoxelGridAroundSegment(segs, crt_model_grid, 1, grid_rot, Vector3(u_min_t, v_min_t, w_min_t), Vector3(u_max_t, v_max_t, w_max_t), voxel_grid);
				voxel_grid_measure = crt_model_grid->ComputeOverlapDiffWithGridUVW(voxel_grid);					
				coverage = crt_model_grid->ComputeOverlapPosWithGridUVW(voxel_grid);																
				
				if (!all_points_in_grid) 
				{
					all_points_in_grid = true;					
					if (voxel_grid->above_u_max > 0 | voxel_grid->below_u_min > 0) { u_max_t += t_step; u_min_t += t_step; all_points_in_grid = false; std::cout << " U++ Above::" << voxel_grid->above_u_max << " Below::" << voxel_grid->below_u_min << std::endl; }					
					if (voxel_grid->below_v_min > 0) { v_min_t += t_step; all_points_in_grid = false; std::cout << " BelowV::" << voxel_grid->below_v_min << std::endl; }
					if (voxel_grid->above_v_max > 0) { v_max_t += t_step; all_points_in_grid = false; std::cout << " AboveV::" << voxel_grid->above_v_max << std::endl; }
					if (voxel_grid->below_w_min > 0) { w_min_t += t_step; all_points_in_grid = false; std::cout << " BelowW::" << voxel_grid->below_w_min << std::endl; }
					if (voxel_grid->above_w_max > 0) { w_max_t += t_step; all_points_in_grid = false; std::cout << " AboveW::" << voxel_grid->above_w_max << std::endl; }
				} 
				else
				{					
					grid_rot += Maths::PiTwo / 24;
					u_min_t = 0; v_min_t = 0; w_min_t = 0;
					u_max_t = 0; v_max_t = 0; w_max_t = 0;
					all_points_in_grid = false;
					//currentlyViewedModelVG = (currentlyViewedModelVG+1) % model_grids.size();
					//std::cout << "Next Grid :: " << currentlyViewedModelVG << std::endl;
					std::cout << "Reset Volume and Apply Rotation:: " << grid_rot << std::endl;
				}		
				*/
				//*****
				std::vector<std::pair<std::pair<bool, float>, Vector3>> * grid_spheres;		
				//grid_spheres = voxel_grid->GetActiveOverlappingCells(crt_model_grid);
				grid_spheres = crt_model_grid->GetAllCellsShowingActive(voxel_grid);
				std::cout << "\nCrt Coverage::" << coverage << " PosMatches::" << voxel_grid_measure.second.first << " NegMatches::" << voxel_grid_measure.second.second << " Diffs::" << voxel_grid_measure.first << " Number of spheres::" << grid_spheres->size() << std::endl;
				KDTreeSphereMesh * sphereMesh = new KDTreeSphereMesh("scene_seg_"+visibility_segment_number+"_grid");
				int g = 0;
				for (auto s : *grid_spheres)
				{
					if (s.first.first) g = 0; else g = 1;
					sphereMesh->AddSphere(s.second, s.first.second, g);
				}
				sphereMesh->ComputeBoundingVolume();				
				sphereMesh->Compile();
				gp->SetShape(sphereMesh);
				gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
				gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("GridMaterial"));
				((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddSegmentGrid("scene_seg_:"+visibility_segment_number, gp);
				scene_change = true;
				accuReset = true;				
			}
		}		

		//Initiate Building of MDP space.
		if (processing_search_request && key.GetKeyState(VK_F1) == KeyboardStates::clicked)
		{
			accuReset = true;
			std::cout << "[Main] Computing Search Space for Structure Fitting - MDP 1 - [START]" << std::endl;
			pcs->ComputeSearchSpace();
			std::vector<FeatureDescStructureGraph::VG_stats*> * vgs = pcs->m_sceneSolution->m_markovDecisionProcess->GetVG_LeafFirstTraversal();
			int idx=0;

			while (pcs->m_sceneSolution->histories->MoreHistory())
			{
				PointCloudSceneSolution::vg_history * crtH = pcs->m_sceneSolution->histories->GetNextHistory();
				pcs->m_sceneSolution->histories->IncHistoriesIndex();
				std::cout << "History :: Best Count=" << crtH->m_vg_history_8->size() << std::endl;				
			}									

			pcs->m_sceneSolution->histories->ResetHistoriesIndex();
			pcs->m_sceneSolution->history = pcs->m_sceneSolution->histories->GetNextHistory();
			pcs->m_sceneSolution->history->SetActiveHistoryLine(8);
			pcs->m_sceneSolution->histories->IncHistoriesIndex();
			pcs->m_sceneToSolve->m_pointCloud->SetInvisibleAllPoints();
			pcs->history_viewing_enabled = true;

			if (!pcs->history_viewing_enabled)
			{
				for (auto vg : *vgs)
				{
					pcs->m_sceneToSolve->ComputeVoxelGridAroundSegment(vg->segs, vg->tgrid, 1, vg->best_rot, vg->best_min_uvw_t, vg->best_max_uvw_t, vg->vgrid);
					std::vector<std::pair<std::pair<bool, float>, Vector3>> * grid_spheres;
					grid_spheres = vg->vgrid->GetActiveOverlappingCellsWithGridBoundaries2(vg->tgrid);
					//grid_spheres = vg->tgrid->GetAllCellsShowingActive(vg->vgrid);
					//grid_spheres = vg->vgrid->GetActiveCells();
					std::cout << vg->tgrid->m_parent->GetInitialNodeId() << ")) Coverage::" << vg->max_coverage << " PosMatches::" << vg->max_pos << " NegMatches::" << vg->max_neg << " Diffs::" << vg->min_diff << " Number of spheres::" << grid_spheres->size() << std::endl;
					//std::cout << "V:" << vg->vgrid->ToString() << std::endl;
					//std::cout << "T:" << vg->tgrid->ToString() << std::endl;
					KDTreeSphereMesh * sphereMesh = new KDTreeSphereMesh("scene_obj_grid_"+idx); idx++;
					int g = 0;
					for (auto s : *grid_spheres)
					{
						if (s.first.first) g = 0; else g = 1;
						sphereMesh->AddSphere(s.second, s.first.second, g);
					}
					sphereMesh->ComputeBoundingVolume();
					sphereMesh->Compile();
					GeometricPrimitive * gp = new GeometricPrimitive();
					gp->SetShape(sphereMesh);
					gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
					gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("GridMaterial"));
					((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddSegmentGrid("scene_obj_:"+idx, gp);

					//Add Model associated to basicspace_pcs.
					std::pair<bool, KDTreeMesh*> shape = pcs->m_trainedModelsDB->GetShape(boost::lexical_cast<std::string, int>(vg->tgrid->m_parent->m_pcs->m_id));
					if (shape.first)
					{				
						GeometricPrimitive * gp1 = new GeometricPrimitive();
						gp1->SetShape(shape.second);					
						gp1->WorldTransform.Reset();

						Matrix3x3 uvw_inverse_scene = vg->vgrid->uvw.GetMatrix();
						uvw_inverse_scene.Transpose();

						Matrix3x3 uvw_inverse_model = vg->tgrid->uvw.GetMatrix();
						uvw_inverse_model.Transpose();					

						Vector3 min_scene_xyz; Vector3 max_scene_xyz;
						Matrix3x3::Product(uvw_inverse_scene, vg->vgrid->min_model_uvw, min_scene_xyz);
						Matrix3x3::Product(uvw_inverse_scene, vg->vgrid->max_model_uvw, max_scene_xyz);
						min_scene_xyz+=vg->vgrid->o_model_xyz;
						max_scene_xyz+=vg->vgrid->o_model_xyz;					

						Vector3 min_model_xyz; Vector3 max_model_xyz;
						Matrix3x3::Product(uvw_inverse_model, vg->tgrid->min_model_uvw, min_model_xyz);
						Matrix3x3::Product(uvw_inverse_model, vg->tgrid->max_model_uvw, max_model_xyz);
						min_model_xyz+=vg->tgrid->o_model_xyz;
						max_model_xyz+=vg->tgrid->o_model_xyz;

						Vector3 segment_origin_model = vg->tgrid->o_model_xyz;
						Vector3 segment_origin_scene = vg->vgrid->o_model_xyz;

						uvw_inverse_scene.Transpose();
						uvw_inverse_model.Transpose();
					
						float scale_factor = Vector3::Distance(max_scene_xyz,min_scene_xyz) / Vector3::Distance(max_model_xyz,min_model_xyz);
						float scale_factor_u = vg->vgrid->grid_cell_spacing_u / vg->tgrid->grid_cell_spacing_u;
						float scale_factor_v = vg->vgrid->grid_cell_spacing_v / vg->tgrid->grid_cell_spacing_v;
						float scale_factor_w = vg->vgrid->grid_cell_spacing_w / vg->tgrid->grid_cell_spacing_w;
						scale_factor = 3.27f;
						Vector3 scale_vector(scale_factor, scale_factor, scale_factor);
						//std::cout << "Scale Vector:: " << scale_vector.ToString() << std::endl;

						Vector3 scale_vector_r(1/scale_factor_u, 1/scale_factor_v, 1/scale_factor_w);

						min_model_xyz *= scale_vector;
						max_model_xyz *= scale_vector;
						Vector3 translation = segment_origin_scene; // - segment_origin_model;

						Vector3 max_min_orientation_model = Vector3::Normalize(max_model_xyz - min_model_xyz);
						Vector3 max_min_orientation_scene = Vector3::Normalize(max_scene_xyz - min_scene_xyz);

						std::vector<Vector3> basis = vg->vgrid->GetBasisVectorsInWorldSpace();
						Vector3 min_loc = basis.at(0);
						Vector3 u_direction_s = Vector3::Normalize(basis.at(1)-min_loc);
						Vector3 v_direction_s = Vector3::Normalize(basis.at(2)-min_loc);
						Vector3 w_direction_s = Vector3::Normalize(basis.at(3)-min_loc);
					
						std::vector<Vector3> basis_m = vg->tgrid->GetBasisVectorsInWorldSpace();
						Vector3 min_loc_m = basis_m.at(0);
						Vector3 u_direction_m = Vector3::Normalize(basis_m.at(1)-min_loc_m);
						Vector3 v_direction_m = Vector3::Normalize(basis_m.at(2)-min_loc_m);
						Vector3 w_direction_m = Vector3::Normalize(basis_m.at(3)-min_loc_m);
					
						float rot_angle_u = Maths::Acos(Vector3::Dot(u_direction_s, u_direction_m));
						Vector3 rot_vector_u = Vector3::Normalize(Vector3::Cross(u_direction_m, u_direction_s));

						float rot_angle_v = Maths::Acos(Vector3::Dot(v_direction_s, v_direction_m));
						Vector3 rot_vector_v = Vector3::Normalize(Vector3::Cross(v_direction_m, v_direction_s));

						float rot_angle_w = Maths::Acos(Vector3::Dot(w_direction_s, w_direction_m));
						Vector3 rot_vector_w = Vector3::Normalize(Vector3::Cross(w_direction_m, w_direction_s));

						translation = segment_origin_scene - segment_origin_model;

						Matrix4x4 tr = Matrix4x4::CreateTranslation(translation);
						Matrix4x4 sc = Matrix4x4::CreateScaling(scale_vector);
					
						Matrix3x3 ro_u_ = Matrix3x3::CreateRotation(rot_vector_u, rot_angle_u);
					
						Transformation ru; ru.Reset(); ru.SetRotation(ro_u_); 
						Vector3 transformed_v = ru.Apply(v_direction_m);
						float angle_vv =  Maths::Acos(Vector3::Dot(transformed_v, v_direction_s));										

						Matrix3x3 ro_v_ = Matrix3x3::CreateRotation(u_direction_s, -angle_vv);										

						Transformation rv; rv.Reset(); rv.SetRotation(ro_v_);
					
						Vector3 transformed_v2 = rv.Apply(transformed_v);
						transformed_v2.Normalize();					 

						Vector3 cross_u_v = Vector3::Cross(u_direction_s, transformed_v2);

						if (Vector3::Dot(cross_u_v, w_direction_s) < 0)	ro_v_ = Matrix3x3::CreateRotation(u_direction_s, angle_vv);					

						Matrix4x4 ro_u = Matrix4x4(ro_u_);
						Matrix4x4 ro_v = Matrix4x4(ro_v_);
										
						Matrix4x4 tr_0r = Matrix4x4::CreateTranslation(-segment_origin_model);
						Matrix4x4 tr_1r = Matrix4x4::CreateTranslation(segment_origin_model);
					
						//----
						KDTreeSphereMesh * sphereMesh_ = new KDTreeSphereMesh("scene_obj_grid_"+idx); idx++;
						//sphereMesh_->AddSphere(min_model_xyz, 1.1, 1);
						//sphereMesh_->AddSphere(max_model_xyz, 1.5, 0);
					
						//sphereMesh_->AddSphere(min_scene_xyz, 1.1, 1);
						//sphereMesh_->AddSphere(max_scene_xyz, 1.5, 0);					

						//std::cout << "O:" << min_loc.ToString() << " U:" << u_direction_s.ToString() << std::endl; // " W:" << w_direction.ToString() << std::endl;
					
						/*sphereMesh_->AddSphere(segment_origin_scene+u_direction_s, 0.4, 1);
						sphereMesh_->AddSphere(segment_origin_scene+u_direction_s*2, 0.4, 1);
						sphereMesh_->AddSphere(segment_origin_scene+u_direction_s*3, 0.4, 1);
						sphereMesh_->AddSphere(segment_origin_scene+u_direction_s*4, 0.4, 1);
						sphereMesh_->AddSphere(segment_origin_scene+u_direction_s*5, 0.4, 1);
						sphereMesh_->AddSphere(segment_origin_scene+u_direction_s*6, 0.4, 1);
					
						sphereMesh_->AddSphere(segment_origin_scene+v_direction_s, 0.4, 0);
						sphereMesh_->AddSphere(segment_origin_scene+v_direction_s*2, 0.4, 1);
						sphereMesh_->AddSphere(segment_origin_scene+v_direction_s*3, 0.4, 0);
						sphereMesh_->AddSphere(segment_origin_scene+v_direction_s*4, 0.4, 1);
						sphereMesh_->AddSphere(segment_origin_scene+v_direction_s*5, 0.4, 0);
						sphereMesh_->AddSphere(segment_origin_scene+v_direction_s*6, 0.4, 1);					

						sphereMesh_->AddSphere(segment_origin_model+u_direction_m, 0.4, 1);
						sphereMesh_->AddSphere(segment_origin_model+u_direction_m*2, 0.4, 1);
						sphereMesh_->AddSphere(segment_origin_model+u_direction_m*3, 0.4, 1);
						sphereMesh_->AddSphere(segment_origin_model+u_direction_m*4, 0.4, 1);
						sphereMesh_->AddSphere(segment_origin_model+u_direction_m*5, 0.4, 1);
						sphereMesh_->AddSphere(segment_origin_model+u_direction_m*6, 0.4, 1);
					
						sphereMesh_->AddSphere(segment_origin_model+v_direction_m, 0.4, 0);
						sphereMesh_->AddSphere(segment_origin_model+v_direction_m*2, 0.4, 1);
						sphereMesh_->AddSphere(segment_origin_model+v_direction_m*3, 0.4, 0);
						sphereMesh_->AddSphere(segment_origin_model+v_direction_m*4, 0.4, 1);
						sphereMesh_->AddSphere(segment_origin_model+v_direction_m*5, 0.4, 0);
						sphereMesh_->AddSphere(segment_origin_model+v_direction_m*6, 0.4, 1);
						*/
						/*sphereMesh_->AddSphere(min_scene_xyz+v_direction, 0.6, 0);
						sphereMesh_->AddSphere(min_scene_xyz+v_direction*2, 0.6, 0);
						sphereMesh_->AddSphere(min_scene_xyz+v_direction*3, 0.6, 0);
						sphereMesh_->AddSphere(min_scene_xyz+v_direction*4, 0.6, 0);

						sphereMesh_->AddSphere(min_scene_xyz+w_direction, 0.8, 1);
						sphereMesh_->AddSphere(min_scene_xyz+w_direction*2, 0.8, 1);
						sphereMesh_->AddSphere(min_scene_xyz+w_direction*3, 0.8, 1);
						sphereMesh_->AddSphere(min_scene_xyz+w_direction*4, 0.8, 1);*/

						//sphereMesh_->AddSphere(Vector3(0,0,0), 0.5, 1);
						/*sphereMesh_->AddSphere(Vector3(1,0,0), 0.5, 1);
						sphereMesh_->AddSphere(Vector3(2,0,0), 0.5, 0);
						sphereMesh_->AddSphere(Vector3(3,0,0), 0.5, 1);
						sphereMesh_->AddSphere(Vector3(4,0,0), 0.5, 0);
						sphereMesh_->AddSphere(Vector3(0,1,0), 0.5, 1);
						sphereMesh_->AddSphere(Vector3(0,2,0), 0.5, 1);
						sphereMesh_->AddSphere(Vector3(0,3,0), 0.5, 1);
						sphereMesh_->AddSphere(Vector3(0,4,0), 0.5, 1);
						sphereMesh_->AddSphere(Vector3(0,0,1), 0.5, 0);
						sphereMesh_->AddSphere(Vector3(0,0,2), 0.5, 0);
						sphereMesh_->AddSphere(Vector3(0,0,3), 0.5, 0);
						sphereMesh_->AddSphere(Vector3(0,0,4), 0.5, 0);*/
						//sphereMesh_->AddSphere(segment_origin_model, 1.0, 1);
						//sphereMesh_->AddSphere(segment_origin_scene, 1.4, 0);
					
						//-------
					
						Matrix4x4 m1, m2, m3, comp;
						((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform.GetTransform(m1);
						gp1->WorldTransform.GetTransform(m2);

						Matrix4x4 worldRotation(((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform.GetRotation());
					
						comp = (tr_1r * ((ro_v * ro_u) * (sc * tr_0r)));					
						
						Matrix4x4 allRotations = worldRotation * ro_v * ro_u;				
						Matrix4x4 ro_v_u = ro_v * ro_u;					

						m3 = worldRotation;

						gp1->WorldTransform.Reset();
						float r[9] = {m3._00, m3._01, m3._02, m3._10, m3._11, m3._12, m3._20, m3._21, m3._22};
						gp1->WorldTransform.SetRotation(Matrix3x3(r));
						gp1->WorldTransform.SetTranslation(Vector3(m3._03, m3._13, m3._23));
					
						Transformation rv2;
						rv2.Reset();
						float r_[9] = {ro_v_u._00, ro_v_u._01, ro_v_u._02, ro_v_u._10, ro_v_u._11, ro_v_u._12, ro_v_u._20, ro_v_u._21, ro_v_u._22};
						rv2.SetRotation(Matrix3x3(r_));
						rv2.SetScaling(scale_vector);
						rv2.SetTranslation(Vector3(0,0,0));
						Vector3 transformed_model_origin = rv2.Apply(segment_origin_model);
					
						for (int i=0; i<shape.second->VertexList.Size(); i++) shape.second->VertexList[i].Position = rv2.Apply(shape.second->VertexList[i].Position);
						for (int i=0; i<shape.second->VertexList.Size(); i++) shape.second->VertexList[i].Position+=segment_origin_scene-transformed_model_origin;
						for (int i = 0; i<shape.second->TriangleList.Size(); i++) { shape.second->TriangleList[i].ComputeEdges(); }					
						shape.second->ComputeBoundingVolume();
						shape.second->Compile();					
					
						sphereMesh_->AddSphere(segment_origin_scene, 1.5, 1);
						//sphereMesh_->AddSphere(transformed_model_origin, 1.0, 1);										

						gp1->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("LitMaterial"));					
						std::string name = "obj_"+boost::lexical_cast<std::string, int>(vg->segs.front());
						((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddModel(name, gp1, tr, ro_u, sc, max_min_orientation_model, comp, tr_0r, tr_1r, scale_vector, translation);

						sphereMesh_->ComputeBoundingVolume();
						sphereMesh_->Compile();
						GeometricPrimitive * gp3 = new GeometricPrimitive();
						gp3->SetShape(sphereMesh_);
						gp3->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
						gp3->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("GridMaterial"));
						((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddSegmentGrid("spheregrid:"+name, gp3);
					}
				}
			}	
			
			((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->SetVisibleAllGrids(false);
			scene_change = true;
			accuReset = true;	
			std::cout << "[Main] Computing Search Space for Structure Fitting - MDP 1 - [FINISH]" << std::endl;
			processing_search_request = false;			
		}
					
		//Reset and show everything
		if (processing_visibility_request && key.GetKeyState(VK_F1) == KeyboardStates::clicked)
		{						
			accuReset = true;
			std::cout << "[Main] Setting All Points to Visible." << std::endl;
			((KDTreePointCloud*)(gp->GetShape()))->SetVisibleAllPoints();
			processing_visibility_request = false;
			accuReset=true;
		}

		//Hide everything
		if (processing_visibility_request && key.GetKeyState(VK_F2) == KeyboardStates::clicked)
		{					
			accuReset = true;
			std::cout << "[Main] Setting All Points to Invisible." << std::endl;
			((KDTreePointCloud*)(gp->GetShape()))->SetInvisibleAllPoints();
			processing_visibility_request = false;			
		}

		//Show Scene OBB
		/*if (processing_visibility_request && key.GetKeyState(VK_F3) == KeyboardStates::clicked)
		{			
			GeometricPrimitive * gp = new GeometricPrimitive();
			KDTreeMesh * q_mesh = new KDTreeMesh();			
			pcs->m_sceneToSolve->ComputeOBB(0.05f);
			ShapeForge::CreateOBBWireFrame(pcs->m_sceneToSolve->m_obb.obb_0, pcs->m_sceneToSolve->m_obb.obb_1, pcs->m_sceneToSolve->m_obb.obb_2, pcs->m_sceneToSolve->m_obb.p_0_min.Distance, pcs->m_sceneToSolve->m_obb.p_0_max.Distance,
																																		pcs->m_sceneToSolve->m_obb.p_1_min.Distance, pcs->m_sceneToSolve->m_obb.p_1_max.Distance,
																																		pcs->m_sceneToSolve->m_obb.p_2_min.Distance, pcs->m_sceneToSolve->m_obb.p_2_max.Distance, q_mesh);
						
			q_mesh->ComputeBoundingVolume();
			q_mesh->Compile();
			gp->SetShape(q_mesh);
			gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
			gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("OBBMaterial"));
			
			//pcs->m_pEnvironment->GetSpace()->PrimitiveList.PushBack(gp);
			((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddOBB("scene", gp);

			std::cout << "OBB_U MaxDist = " << pcs->m_sceneToSolve->m_obb.p_0_max.Distance << " MinDist=" << pcs->m_sceneToSolve->m_obb.p_0_min.Distance << " N=" << pcs->m_sceneToSolve->m_obb.obb_0.ToString() << std::endl;
			std::cout << "OBB_V MaxDist = " << pcs->m_sceneToSolve->m_obb.p_1_max.Distance << " MinDist=" << pcs->m_sceneToSolve->m_obb.p_1_min.Distance << " N=" << pcs->m_sceneToSolve->m_obb.obb_1.ToString() << std::endl;
			std::cout << "OBB_W MaxDist = " << pcs->m_sceneToSolve->m_obb.p_2_max.Distance << " MinDist=" << pcs->m_sceneToSolve->m_obb.p_2_min.Distance << " N=" << pcs->m_sceneToSolve->m_obb.obb_2.ToString() << std::endl;

			processing_visibility_request = false;
		}*/


		if (processing_visibility_request && key.GetKeyState(VK_F3) == KeyboardStates::clicked)
		{
			if (current_segment_view_idx==0)
			{
				sorted_segs = new std::vector<std::vector<int>>();
				std::vector<std::vector<int>> sorted = pcs->m_sceneToSolve->m_structureGraph->GetNodesSortedBySize();
				for (auto s : sorted) sorted_segs->push_back(s);
			}
			
			while (current_segment_view_idx < sorted_segs->size())
			{
				((KDTreePointCloud*)(gp->GetShape()))->AddToVisiblePointsWithSegmentIds(sorted_segs->at(current_segment_view_idx));
				current_segment_view_idx++;
				//std::cout << "[Main] NEXT - Setting Points with SegmentID " << sorted_segs->at(current_segment_view_idx)  << " to Visible." << std::endl;
				accuReset=true;
			}
			processing_visibility_request = false;
		}

		/*if (processing_visibility_request && key.GetKeyState(VK_F4) == KeyboardStates::clicked)
		{
			if (current_segment_view_idx==0)
			{
				sorted_segs = new std::vector<std::vector<int>>();
				std::vector<std::vector<int>> sorted = pcs->m_sceneToSolve->m_structureGraph->GetNodesSortedBySize();
				for (auto s : sorted) sorted_segs->push_back(s);
			}
			
			if (current_segment_view_idx >= 0)
			{
				((KDTreePointCloud*)(gp->GetShape()))->AddToVisiblePointsWithSegmentIds(sorted_segs->at(current_segment_view_idx));
				current_segment_view_idx--;				
				//std::cout << "[Main] PREV - Setting Points with SegmentID " << sorted_segs->at(current_segment_view_idx)  << " to Visible." << std::endl;
				accuReset=true;
			}
			processing_visibility_request = false;
		}*/

		if (processing_visibility_request && key.GetKeyState(VK_F4) == KeyboardStates::clicked)		
		/*{
			std::vector<int> undefined;
			for (auto s : *(pcs->m_sceneToSolve->m_segmentsGraph->m_surfaceSegments))
			{
				if (s.second->shape_primitive == SegmentsGraph::Shape::unknown) undefined.push_back(s.first);
			}

			for (auto s : undefined)
			{
				((KDTreePointCloud*)(gp->GetShape()))->AddToVisiblePointsWithSegmentIds(undefined);
			}			
		}*/
		{
			std::vector<int> trunks; 
			trunks.push_back(48290);
			trunks.push_back(54813);
			trunks.push_back(55334);
			trunks.push_back(56568);
			trunks.push_back(56641);
			trunks.push_back(57132);
			trunks.push_back(58383);

			trunks.push_back(55514);
			trunks.push_back(215576);
			trunks.push_back(57592);
			trunks.push_back(53137);
			trunks.push_back(53220);
			trunks.push_back(57273);

			trunks.push_back(56827);
			trunks.push_back(215588);
			trunks.push_back(56951);

			trunks.push_back(205889);

			trunks.push_back(29999);
			trunks.push_back(215543);
			
			std::vector<int> leaves;
			for (auto t : trunks)
			{
				std::cout << "Trunk @ " << t << std::endl;
				pcs->m_sceneToSolve->m_segmentsGraph->ComputeOBB(t, 0.f);
				((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithSegmentId(t);
				SegmentsGraph::OBB obb = pcs->m_sceneToSolve->m_segmentsGraph->m_surfaceSegments->at(t)->obb;				

				if (t != 215543)
				{
					/*
					pcs->m_sceneToSolve->m_segmentsGraph->m_surfaceSegments->at(t)->obb.p_2_min.Distance -= 30;
					pcs->m_sceneToSolve->m_segmentsGraph->m_surfaceSegments->at(t)->obb.p_2_max.Distance += 30;
					pcs->m_sceneToSolve->m_segmentsGraph->m_surfaceSegments->at(t)->obb.p_1_min.Distance -= 10;
					pcs->m_sceneToSolve->m_segmentsGraph->m_surfaceSegments->at(t)->obb.p_1_max.Distance += 10;
					pcs->m_sceneToSolve->m_segmentsGraph->m_surfaceSegments->at(t)->obb.p_0_min.Distance -= 10;
					pcs->m_sceneToSolve->m_segmentsGraph->m_surfaceSegments->at(t)->obb.p_0_max.Distance += 10;
					*/
					obb.p_2_min.Distance-=30;
					obb.p_2_max.Distance+=30;
					obb.p_1_min.Distance-=10;
					obb.p_1_max.Distance+=10;
					obb.p_0_min.Distance-=10;
					obb.p_0_max.Distance+=10;
				}
				else
				{
					obb.p_2_min.Distance-=10;
					obb.p_2_max.Distance+=10;
					obb.p_1_min.Distance-=30;
					obb.p_1_max.Distance+=30;
					obb.p_0_min.Distance-=10;
					obb.p_0_max.Distance+=10;

					/*
					pcs->m_sceneToSolve->m_segmentsGraph->m_surfaceSegments->at(t)->obb.p_2_min.Distance -= 10;
					pcs->m_sceneToSolve->m_segmentsGraph->m_surfaceSegments->at(t)->obb.p_2_max.Distance += 10;
					pcs->m_sceneToSolve->m_segmentsGraph->m_surfaceSegments->at(t)->obb.p_1_min.Distance -= 30;
					pcs->m_sceneToSolve->m_segmentsGraph->m_surfaceSegments->at(t)->obb.p_1_max.Distance += 30;
					pcs->m_sceneToSolve->m_segmentsGraph->m_surfaceSegments->at(t)->obb.p_0_min.Distance -= 10;
					pcs->m_sceneToSolve->m_segmentsGraph->m_surfaceSegments->at(t)->obb.p_0_max.Distance += 10;
					*/
				}
							
				int new_edge_id = 500000;
				leaves.clear();
				for (auto s : *(pcs->m_sceneToSolve->GetSegmentsGraph()->m_edgeSegments))
				{
					if (pcs->m_sceneToSolve->GetSegmentsGraph()->InOBBPercentage(obb, s.second) > 0.f)
					{
						for (auto pnt : *(s.second->points)) pcs->m_sceneToSolve->m_pointCloud->m_pointList.at(pnt).m_segmentId = new_edge_id;						
						std::cout << "Adding Edge Segment " << s.first << std::endl;
						leaves.push_back(s.first);
					}
				}
								
				((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithSegmentId(new_edge_id);
				new_edge_id+=1;
				accuReset=true;
			}
			accuReset=true;
			processing_visibility_request = false;
		}

		//Show/Hide Models fitted in the scene
		if (processing_visibility_request && key.GetKeyState(VK_F9) == KeyboardStates::clicked)
		{
			/*
			try {
				accuReset = true;
				int obbcount = 0;
				processing_visibility_request = false;				
				SegmentsGraph * sg = pcs->m_sceneToSolve->GetSegmentsGraph();
				for (auto seg_it = pcs->m_sceneToSolve->GetStructureGraph()->m_nodes->begin(); seg_it != pcs->m_sceneToSolve->GetStructureGraph()->m_nodes->end(); seg_it++)
				{
					std::string segment_id = boost::lexical_cast<std::string, int>(seg_it->first);
					if (!((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->SetVisibleOBB("S"+segment_id, true))
					{
						GeometricPrimitive * gp = new GeometricPrimitive();
						KDTreeMesh * q_mesh = new KDTreeMesh();
						ShapeForge::CreateOBBWireFrame(sg->m_surfaceSegments->at(seg_it->first)->obb.obb_0, sg->m_surfaceSegments->at(seg_it->first)->obb.obb_1, sg->m_surfaceSegments->at(seg_it->first)->obb.obb_2,
														sg->m_surfaceSegments->at(seg_it->first)->obb.p_0_min.Distance, sg->m_surfaceSegments->at(seg_it->first)->obb.p_0_max.Distance,
														sg->m_surfaceSegments->at(seg_it->first)->obb.p_1_min.Distance, sg->m_surfaceSegments->at(seg_it->first)->obb.p_1_max.Distance,
														sg->m_surfaceSegments->at(seg_it->first)->obb.p_2_min.Distance, sg->m_surfaceSegments->at(seg_it->first)->obb.p_2_max.Distance, q_mesh);
						
						q_mesh->ComputeBoundingVolume();
						q_mesh->Compile();
						gp->SetShape(q_mesh);
						gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
						gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("OBBMaterial"));
												
						((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddOBB("S"+segment_id, gp);
						obbcount++;
					}
				}
				std::cout << "[Main] Showing " << obbcount << " OBBs of ALL Surface Segments"<< std::endl;

			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN <S>] Boost::Bad_lexical_cast :" << visibility_segment_number << std::endl;
			}
			*/
			
			try {				
				accuReset = true;
				processing_visibility_request = false;
				models_visible = !models_visible;
				std::cout << "[Main] Hiding/Showing Fitted Models"<< std::endl;
				((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->SetVisibleAllModels(models_visible);

			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN <S>] Boost::Bad_lexical_cast :" << visibility_segment_number << std::endl;
			}

		}

		//Hide/Show all Grids
		if (processing_visibility_request && key.GetKeyState(VK_F10) == KeyboardStates::clicked)
		{
			try {				
				accuReset = true;
				processing_visibility_request = false;
				grids_visible = !grids_visible;
				std::cout << "[Main] Hiding/Showing Fitted Grids"<< std::endl;
				((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->SetVisibleAllGrids(grids_visible);

			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN <S>] Boost::Bad_lexical_cast :" << visibility_segment_number << std::endl;
			}
		}

		if (processing_visibility_request && key.GetKeyState(VK_F11) == KeyboardStates::clicked)
		{

		}

		if (processing_visibility_request && key.GetKeyState(VK_F12) == KeyboardStates::clicked)
		{

		}		

		//Show Surface Points
		if (processing_visibility_request && key.GetKeyState(VK_F5) == KeyboardStates::clicked)
		{					
			accuReset = true;			
			((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithTypeId(4);
			processing_visibility_request = false;			
		}

		//Show Edge Points
		if (processing_visibility_request && key.GetKeyState(VK_F6) == KeyboardStates::clicked)
		{					
			accuReset = true;			
			((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithTypeId(3);
			processing_visibility_request = false;
		}

		//Show Corner Points
		if (processing_visibility_request && key.GetKeyState(VK_F7) == KeyboardStates::clicked)
		{					
			accuReset = true;			
			((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithTypeId(5);
			processing_visibility_request = false;
		}

		//Show Seed Points
		if (processing_visibility_request && key.GetKeyState(VK_F8) == KeyboardStates::clicked)
		{					
			accuReset = true;			
			((KDTreePointCloud*)(gp->GetShape()))->SetInvisibleAllPointsExceptSeeds();
			processing_visibility_request = false;
		}		

		//View Corner Detection Process
		if (processing_visibility_request && key.GetKeyState('C') == KeyboardStates::clicked)
		{				
			accuReset = true;
			((KDTreePointCloud*)(gp->GetShape()))->ComputeCornerTypes(std::vector<int>(), true);
			processing_visibility_request = false;
		}

		//View All Points in One Colour
		if (processing_visibility_request && key.GetKeyState('R') == KeyboardStates::clicked)
		{
			accuReset = true;
			if (restore)
			{
				((KDTreePointCloud*)(gp->GetShape()))->RestoreAllPointsSegmentIdsFromCache();
				restore = false;
			}
			else
			{
				((KDTreePointCloud*)(gp->GetShape()))->SetAllPointsInOneSegment();
				restore = true;
			}
			processing_visibility_request = false;
		}

		//Switch State and wait for input of surface segment number
		if (processing_visibility_request && !processing_visibility_segmentid && key.GetKeyState('S') == KeyboardStates::clicked)
		{							
			processing_visibility_segmentid = true;
			std::cout << "[Main] Input Segment Number ... " << std::endl;
			visibility_segment_number.clear();
		}

		//Switch State and wait for input of OBB segment number 
		if (processing_visibility_request && !processing_visibility_obbsegmentid && key.GetKeyState('O') == KeyboardStates::clicked)
		{							
			processing_visibility_obbsegmentid = true;
			std::cout << "[Main] Input OBB Segment Number ... " << std::endl;
			visibility_segment_number.clear();
		}

		//Show ONLY points with segmentid
		if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('S') == KeyboardStates::clicked)
		{					
			try {
				//std::cout << "Pressed Segment=" << visibility_segment_number << "." << std::endl;
				accuReset = true;
				processing_visibility_segmentid = false;											
				processing_visibility_request = false;
				int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
				SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
				if (chosen_segment != NULL)
				{					
					std::cout << "[Main] Showing Surface Segment with id " << segment_id << std::endl;
					((KDTreePointCloud*)(gp->GetShape()))->SetInvisibleAllPointsExceptForSegmentId(segment_id);
					currentlyViewedSegment = segment_id;
					chosen_segment->internalGraphSelectionIndex = -1;							
				} 
				else
				{
					SegmentsGraph::EdgeSegment * chosen_edge_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->EdgeSegmentWithId(segment_id);
					if (chosen_edge_segment != NULL)
					{
						std::cout << "[Main] Showing Edge Segment with id " << segment_id << std::endl;			
						((KDTreePointCloud*)(gp->GetShape()))->SetInvisibleAllPointsExceptForSegmentId(segment_id);
						//currentlyViewedSegment = segment_id;
						//chosen_segment->internalGraphSelectionIndex = -1;							
					}
					std::cout << "[EXCEPTION THROWN <S>] Segment not found :" << visibility_segment_number << std::endl;
				}

			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN <S>] Boost::Bad_lexical_cast :" << visibility_segment_number << std::endl;
			} 

		}

		//Show OBB of segment 
		if (processing_visibility_request && processing_visibility_obbsegmentid && key.GetKeyState('O') == KeyboardStates::clicked)
		{					
			try {
				//std::cout << "Pressed Segment=" << visibility_segment_number << "." << std::endl;
				accuReset = true;
				processing_visibility_obbsegmentid = false;
				processing_visibility_request = false;
				int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
				SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
				if (chosen_segment != NULL)
				{
					std::cout << "[Main] Showing OBB of Surface Segment with id " << segment_id << std::endl;
					if (!((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->SetVisibleOBB("S"+visibility_segment_number, true))
					{
						GeometricPrimitive * gp = new GeometricPrimitive();
						KDTreeMesh * q_mesh = new KDTreeMesh();						
						ShapeForge::CreateOBBWireFrame(chosen_segment->obb.obb_0, chosen_segment->obb.obb_1, chosen_segment->obb.obb_2, chosen_segment->obb.p_0_min.Distance, chosen_segment->obb.p_0_max.Distance,
																																		chosen_segment->obb.p_1_min.Distance, chosen_segment->obb.p_1_max.Distance,
																																		chosen_segment->obb.p_2_min.Distance, chosen_segment->obb.p_2_max.Distance, q_mesh);
						
						std::cout << "OBB_U MaxDist = " << chosen_segment->obb.p_0_max.Distance << " MinDist=" << chosen_segment->obb.p_0_min.Distance << " N=" << chosen_segment->obb.obb_0.ToString() << std::endl;
						std::cout << "OBB_V MaxDist = " << chosen_segment->obb.p_1_max.Distance << " MinDist=" << chosen_segment->obb.p_1_min.Distance << " N=" << chosen_segment->obb.obb_1.ToString() << std::endl;
						std::cout << "OBB_W MaxDist = " << chosen_segment->obb.p_2_max.Distance << " MinDist=" << chosen_segment->obb.p_2_min.Distance << " N=" << chosen_segment->obb.obb_2.ToString() << std::endl;
						q_mesh->ComputeBoundingVolume();
						q_mesh->Compile();
						gp->SetShape(q_mesh);
						gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
						gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("OBBMaterial"));
												
						((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddOBB("S"+visibility_segment_number, gp);

					}
				}
				else
				{
					SegmentsGraph::EdgeSegment * chosen_edge_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->EdgeSegmentWithId(segment_id);
					if (chosen_edge_segment != NULL)
					{
						std::cout << "[Main] NO IMPLEMENTATION - Showing OBB of Edge Segment with id " << segment_id << std::endl;								
					
					}
					std::cout << "[EXCEPTION THROWN <S>] Segment not found :" << visibility_segment_number << std::endl;
				}

			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN <S>] Boost::Bad_lexical_cast :" << visibility_segment_number << std::endl;
			} 
		}

		//Show OBB of internal nodes in segment 
		if (processing_visibility_request && processing_visibility_obbsegmentid && key.GetKeyState('I') == KeyboardStates::clicked)
		{					
			try {
				//std::cout << "Pressed Segment=" << visibility_segment_number << "." << std::endl;
				accuReset = true;
				processing_visibility_obbsegmentid = false;											
				processing_visibility_request = false;
				int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
				SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
				
				if (chosen_segment != NULL)
				{				
					if (chosen_segment->shape_primitive != SegmentsGraph::compound) 
					{
						std::cout << "[Main] Primitive of Surface Segment with id " << segment_id << " is not compound" << std::endl;						
					}
					else
					{
						std::cout << "[Main] Showing OBB of Internal Segments for Surface Segment with id " << segment_id << std::endl;
						StructureGraph * internalStrGraph = chosen_segment->internalStrGraph;
						SegmentsGraph * internalSegGraph = chosen_segment->internalSegGraph;
						for (auto nodes_it = internalStrGraph->m_nodes->begin(); nodes_it != internalStrGraph->m_nodes->end(); nodes_it++)
						{
							if (internalStrGraph->IsNodeMerged(nodes_it->first)) continue;
							std::string node_id = boost::lexical_cast<std::string, int>(nodes_it->first);
							if (!((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->SetVisibleOBB("S"+visibility_segment_number+"I"+node_id, true))
							{
								GeometricPrimitive * gp = new GeometricPrimitive();
								KDTreeMesh * q_mesh = new KDTreeMesh();						
								ShapeForge::CreateOBBWireFrame(internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.obb_0, internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.obb_1, internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.obb_2, 
																internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.p_0_min.Distance, internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.p_0_max.Distance,
																internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.p_1_min.Distance, internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.p_1_max.Distance,
																internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.p_2_min.Distance, internalSegGraph->m_surfaceSegments->at(nodes_it->first)->obb.p_2_max.Distance, q_mesh);
						
								q_mesh->ComputeBoundingVolume();
								q_mesh->Compile();
								gp->SetShape(q_mesh);
								gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
								gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("OBBMaterial"));
												
								((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddOBB("S"+visibility_segment_number+"I"+node_id, gp);
						}
						}
					}
				} 				

			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN <S>] Boost::Bad_lexical_cast :" << visibility_segment_number << std::endl;
			} 
		}

		//Hide OBB of segment 
		if (processing_visibility_request && processing_visibility_obbsegmentid && key.GetKeyState('H') == KeyboardStates::clicked)
		{					
			try {
				//std::cout << "Pressed Segment=" << visibility_segment_number << "." << std::endl;
				accuReset = true;
				processing_visibility_obbsegmentid = false;											
				processing_visibility_request = false;
				int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
				SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
				if (chosen_segment != NULL)
				{					
					std::cout << "[Main] Hiding OBB of Surface Segment with id " << segment_id << std::endl;			
					((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->SetVisibleOBB("S"+visibility_segment_number, false);					
				} 
				else
				{
					SegmentsGraph::EdgeSegment * chosen_edge_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->EdgeSegmentWithId(segment_id);
					if (chosen_edge_segment != NULL)
					{
						std::cout << "[Main] NO IMPLEMENTATION - Showing OBB of Edge Segment with id " << segment_id << std::endl;								
					
					}
					std::cout << "[EXCEPTION THROWN <S>] Segment not found :" << visibility_segment_number << std::endl;
				}

			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN <S>] Boost::Bad_lexical_cast :" << visibility_segment_number << std::endl;
			} 

		}

		//Show ONLY points of the mostly visible surface
		if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('O') == KeyboardStates::clicked)
		{					
			try {
				//std::cout << "Pressed Segment=" << visibility_segment_number << "." << std::endl;
				accuReset = true;
				processing_visibility_segmentid = false;											
				processing_visibility_request = false;
				int segment_id = pcs->m_sceneToSolve->m_mostVisibileSurface;
				std::cout << "[Main] Max Visible Surface (MAX VisibilitY) set to id " << segment_id << std::endl;
				SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
				if (chosen_segment != NULL)
				{					
					std::cout << "[Main] Showing Surface Segment (MAX VisibilitY) with id " << segment_id << std::endl;
					((KDTreePointCloud*)(gp->GetShape()))->SetInvisibleAllPointsExceptForSegmentId(segment_id);
					currentlyViewedSegment = segment_id;
					chosen_segment->internalGraphSelectionIndex = -1;							
				} 
				else
				{
					SegmentsGraph::EdgeSegment * chosen_edge_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->EdgeSegmentWithId(segment_id);
					if (chosen_edge_segment != NULL)
					{
						std::cout << "[Main] Showing Edge Segment with id " << segment_id << std::endl;			
						((KDTreePointCloud*)(gp->GetShape()))->SetInvisibleAllPointsExceptForSegmentId(segment_id);
						//currentlyViewedSegment = segment_id;
						//chosen_segment->internalGraphSelectionIndex = -1;							
					}
					std::cout << "[EXCEPTION THROWN <S>] Segment not found :" << visibility_segment_number << std::endl;
				}

			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN <S>] Boost::Bad_lexical_cast :" << visibility_segment_number << std::endl;
			} 

		}

		//Hide points with segmentid
		if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('H') == KeyboardStates::clicked)
		{					
			try {				
				accuReset = true;
				processing_visibility_segmentid = false;											
				processing_visibility_request = false;
				int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
				SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);				
				if (chosen_segment != NULL)
				{
					std::cout << "[Main] Not Showing Segment with id " << segment_id << std::endl;			
					((KDTreePointCloud*)(gp->GetShape()))->SetInvisiblePointsWithSegmentId(segment_id);							
				}
				else
				{
					std::cout << "[EXCEPTION THROWN <H>] Segment not found :" << visibility_segment_number << std::endl;
				}
			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN <H>] Boost::Bad_lexical_cast" << std::endl;
			}
		}

		//Show also points with segmentid
		if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('A') == KeyboardStates::clicked)
		{					
			try {				
				accuReset = true;
				processing_visibility_segmentid = false;											
				processing_visibility_request = false;
				int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
				SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);				
				if (chosen_segment != NULL)
				{
					std::cout << "[Main] Adding Surface Segment with id " << segment_id << std::endl;			
					((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithSegmentId(segment_id);							
				}
				else
				{
					SegmentsGraph::EdgeSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->EdgeSegmentWithId(segment_id);
					if (chosen_segment != NULL)
					{
						std::cout << "[Main] Adding Edge Segment with id " << segment_id << std::endl;			
						((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithSegmentId(segment_id);							
					}
					else
					{
						std::cout << "[EXCEPTION THROWN <A>] Segment not found :" << visibility_segment_number << std::endl;
					}
				}
			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN <A>] Boost::Bad_lexical_cast" << std::endl;
			}
		}

		//Show directly connected segments
		if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('D') == KeyboardStates::clicked)
		{								
			try {
				accuReset = true;
				processing_visibility_segmentid = false;
				processing_visibility_request = false;
				int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
				SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);				
				if (chosen_segment != NULL)
				{					
					std::vector<int> connected_segments = pcs->m_sceneToSolve->GetStructureGraph()->DirectlyConnectedSegmentsAtAngle(segment_id, 0.95f, 0.05f);			
					std::cout << "[Main] Showing " << connected_segments.size() << " segments connected DIRECTLY (at angle) with segment # " << segment_id << std::endl;			
					((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithSegmentIds(connected_segments);
					currentlyViewedSegment = segment_id;
					chosen_segment->internalGraphSelectionIndex = -1;							
				}
				else
				{
					std::cout << "[EXCEPTION THROWN <D>] Segment not found :" << visibility_segment_number << std::endl;
				}
			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN <D>] Boost::Bad_lexical_cast" << std::endl;
			}
		}

		//Show segments intersecting the OBB of segment.
		if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('Z') == KeyboardStates::clicked)
		{								
			try {
				accuReset = true;
				processing_visibility_segmentid = false;
				processing_visibility_request = false;
				int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
				SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
				
				if (chosen_segment != NULL)
				{					
					SegmentsGraph::OBB obb = chosen_segment->obb;
					obb.p_0_min.Distance-=2.f;
					obb.p_0_max.Distance+=2.f;
					for (auto nextSeg : *(pcs->m_sceneToSolve->GetStructureGraph()->m_nodes))
					{
						float p = pcs->m_sceneToSolve->GetSegmentsGraph()->InOBBPercentage(obb, nextSeg.second->segment);
						if (p > 0)
						{
							//std::cout << "[Main] Showing " << connected_segments.size() << " segments connected DIRECTLY (at angle) with segment # " << segment_id << std::endl;			
							((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithSegmentId(nextSeg.first);							
						}
					}
					currentlyViewedSegment = segment_id;
					chosen_segment->internalGraphSelectionIndex = -1;
				}
				else
				{
					std::cout << "[EXCEPTION THROWN <D>] Segment not found :" << visibility_segment_number << std::endl;
				}
			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN <D>] Boost::Bad_lexical_cast" << std::endl;
			}
		}


		//Merge Surface States
		if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('M') == KeyboardStates::clicked)
		{					
			try {
				accuReset = true;
				processing_visibility_segmentid = false;
				processing_visibility_request = false;
				int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
				SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
				if (chosen_segment != NULL)
				{
					bool done = false;
					//Check if we first have to merge the internal states of this segment
					if (chosen_segment->internalSegGraph == NULL)
					{
						pcs->m_sceneToSolve->MergeStates(true, segment_id, 2, 0.95f, 0.05f, 0);			
						std::cout << "[Main] Merging (Level 0) segments connected DIRECTLY (at angle) with segment #" << segment_id << std::endl;			
						currentlyViewedSegment = segment_id;
						chosen_segment->internalGraphSelectionIndex = -1;
						done = true;
					}

					if (!done)
					{
						if (chosen_segment->internalSegGraph->m_surfaceSegments->size() > 1)
						{
							pcs->m_sceneToSolve->MergeStates(true, segment_id, 2, 0.95f, 0.05f, 1);			
							std::cout << "[Main] Merging (Level 1) segments connected DIRECTLY (at angle) in segment #" << segment_id << std::endl;
							currentlyViewedSegment = segment_id;
							chosen_segment->internalGraphSelectionIndex = -1;
						}
						else
						{
							pcs->m_sceneToSolve->MergeStates(true, segment_id, 2, 0.95f, 0.05f, 0);			
							std::cout << "[Main] Merging (Level 0) segments connected DIRECTLY (at angle) with segment #" << segment_id << std::endl;			
							currentlyViewedSegment = segment_id;
							chosen_segment->internalGraphSelectionIndex = -1;
						}
					}
				}
				else
				{
					std::cout << "[EXCEPTION THROWN <M>] Segment not found :" << visibility_segment_number << std::endl;
				}
			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN <M>] Boost::Bad_lexical_cast" << std::endl;
			}
		}

		//Use G to merge automatically global graph
		if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('G') == KeyboardStates::clicked)
		{					
			try
			{
				accuReset = true;
				processing_visibility_segmentid = false;
				processing_visibility_request = false;
				int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
				SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
				if (chosen_segment != NULL)
				{
					std::cout << "[Main] Merging (AUTO) level 0 segments connected DIRECTLY (at angle) to segment " << segment_id << std::endl;
					pcs->m_sceneToSolve->MergeStates(false, segment_id, 2, 0.95f, 0.05f, 0);
				}
				else
				{
					std::cout << "[EXCEPTION THROWN <M>] Segment not found :" << visibility_segment_number << std::endl;
				}
			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN <M>] Boost::Bad_lexical_cast" << std::endl;
			}
		}
		
		//Use B to extract and show boundaries - press B again to view the surface segments not associated to boundaries. 
		if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('B') == KeyboardStates::clicked)
		{	
			accuReset = true;
			processing_visibility_segmentid = false;
			processing_visibility_request = false;												
			if (b_nodes.size() == 0)
			{				
				std::cout << "[Main] Extracting Boundaries." << std::endl;
				pcs->m_sceneToSolve->m_structureGraph->PromoteInternalNodesToLevel0();
				pcs->m_sceneToSolve->MergeStates(false, 0, 2, 0.95f, 0.05f, 0);
				//pcs->m_sceneToSolve->DrawSceneGraph(pcs->m_sceneToSolve->m_structureGraph, pcs->m_sceneToSolve->m_pointCloud->GetName(), 1.2f);	//This one should discard obb_overlap info since > 1
				pcs->m_sceneToSolve->DrawSceneGraph(pcs->m_sceneToSolve->m_structureGraph, pcs->m_sceneToSolve->m_pointCloud->GetName(), 0.1f);	//This one should print only those edge that overlap by 10%
				PointCloudSceneSolution pcss(pcs->m_sceneToSolve, pcs->m_trainedModelsDB);
				pcss.SearchBoundaries(pcss.m_markovDecisionProcess->m_root, 0.95f, 0.2f, 20.0f);
				std::cout << "Boundary Surface Segments :: " << std::endl;
				b_nodes = pcss.m_markovDecisionProcess->GetBoundaryNodes();
				std::cout << "b_nodes size::" << b_nodes.size() << std::endl;
				((KDTreePointCloud*)(gp->GetShape()))->SetInvisibleAllPoints();
				((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithSegmentIds(b_nodes);
			}
			else
			{
				std::map<int, int> segments_map;				
				((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithTypeId(4, segments_map);
				for (auto b_node:b_nodes) ((KDTreePointCloud*)(gp->GetShape()))->SetInvisiblePointsWithSegmentId(b_node);

				std::map<int, std::vector<int>> ordered_segments;
				for (auto segments_map_it : segments_map) ordered_segments[segments_map_it.second].push_back(segments_map_it.first);

				std::cout << "Visible Segments Count = " << ordered_segments.size() - b_nodes.size() << std::endl; 
				bool skip = false;
				for (auto segs : ordered_segments)
				{
					if (segs.first > 24)
					{
						for (auto s : segs.second)
						{
							for (auto b_node:b_nodes) if (b_node == s) { skip=true; continue; }
							if (!skip) std::cout << "[KDTreePointCloud] SegID::" << s << " #" << segs.first << std::endl; 
						}
					}
					skip=false;
				}
				b_nodes.clear();
			}			
		}		

		//Show all directly connected surfaces
		if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('F') == KeyboardStates::clicked)
		{					
			try {
				accuReset = true;
				processing_visibility_segmentid = false;				
				processing_visibility_request = false;
				int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);
				SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);
				if (chosen_segment != NULL)
				{
					std::vector<int> connected_segments = pcs->m_sceneToSolve->GetStructureGraph()->AllDirectlyConnectedSegments(segment_id);
					std::cout << "[Main] Showing " << connected_segments.size() << " segments connected DIRECTLY with segment # " << segment_id << std::endl;
					((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithSegmentIds(connected_segments);
					currentlyViewedSegment = segment_id;
					chosen_segment->internalGraphSelectionIndex = -1;
				}
			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN] Boost::Bad_lexical_cast" << std::endl;
			}
		}

		//Show transitively connected surfaces at angle
		if (processing_visibility_request && processing_visibility_segmentid && key.GetKeyState('T') == KeyboardStates::clicked)
		{					
			try {
				accuReset = true;
				processing_visibility_segmentid = false;											
				processing_visibility_request = false;
				int segment_id = boost::lexical_cast<int, std::string>(visibility_segment_number);			
				SegmentsGraph::SurfaceSegment * chosen_segment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(segment_id);				
				if (chosen_segment != NULL)
				{
					std::vector<int> connected_segments = pcs->m_sceneToSolve->GetStructureGraph()->TransitivelyConnectedSegmentsAtAngle(segment_id, 0.95f, 0.05f);
					std::cout << "[Main] Showing " << connected_segments.size() << " segments connected TRANSITIVELY (at angle) with segment # " << segment_id << std::endl;
					((KDTreePointCloud*)(gp->GetShape()))->SetInvisibleAllPoints();
					boost::this_thread::sleep(boost::posix_time::milliseconds(120));
					/*Used only to create video for mnajdra*/
					std::vector<int> filtered_segs;
					for (auto connected_segment : connected_segments)
					{
						//if (pcs->m_sceneToSolve->GetSegmentsGraph()->m_surfaceSegments->at(connected_segment)->points->size() < 350)												
						std::cout << pcs->m_sceneToSolve->GetSegmentsGraph()->m_surfaceSegments->at(connected_segment)->obb.GetVolume() << std::endl;
						if (pcs->m_sceneToSolve->GetSegmentsGraph()->m_surfaceSegments->at(connected_segment)->obb.GetVolume() < 120)
						{							
							//if (Maths::Abs(Vector3::Dot(pcs->m_sceneToSolve->GetSegmentsGraph()->m_surfaceSegments->at(connected_segment)->obb.obb_0, chosen_segment->obb.obb_0)) > 0.2f)
							{
								filtered_segs.push_back(connected_segment);
								//std::cout << pcs->m_sceneToSolve->GetSegmentsGraph()->m_surfaceSegments->at(connected_segment)->obb.obb_0.ToString() << std::endl;
								((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithSegmentId(connected_segment);
								boost::this_thread::sleep(boost::posix_time::milliseconds(5));
							}
						}
					}
					//((KDTreePointCloud*)(gp->GetShape()))->SetVisiblePointsWithSegmentIds(connected_segments);
					currentlyViewedSegment = segment_id;
					chosen_segment->internalGraphSelectionIndex = -1;							
				}
			} catch (boost::bad_lexical_cast) {
				std::cout << "[EXCEPTION THROWN] Boost::Bad_lexical_cast" << std::endl;
			}
		}		

		if (processing_visibility_request && (processing_visibility_segmentid || processing_visibility_obbsegmentid))
		{
			if (key.GetKeyState('0') == KeyboardStates::clicked) visibility_segment_number.append("0");
			if (key.GetKeyState('1') == KeyboardStates::clicked) visibility_segment_number.append("1");
			if (key.GetKeyState('2') == KeyboardStates::clicked) visibility_segment_number.append("2");
			if (key.GetKeyState('3') == KeyboardStates::clicked) visibility_segment_number.append("3");
			if (key.GetKeyState('4') == KeyboardStates::clicked) visibility_segment_number.append("4");
			if (key.GetKeyState('5') == KeyboardStates::clicked) visibility_segment_number.append("5");
			if (key.GetKeyState('6') == KeyboardStates::clicked) visibility_segment_number.append("6");
			if (key.GetKeyState('7') == KeyboardStates::clicked) visibility_segment_number.append("7");
			if (key.GetKeyState('8') == KeyboardStates::clicked) visibility_segment_number.append("8");
			if (key.GetKeyState('9') == KeyboardStates::clicked) visibility_segment_number.append("9");
		}


		//---Iterate through primitive segments in level 0 surface segments 
		if (!processing_visibility_request && !processing_pointcloud_switch_request && key.GetKeyState('N') == KeyboardStates::clicked)
		{					
			SegmentsGraph::SurfaceSegment * crtSegment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(currentlyViewedSegment);
			if (crtSegment != NULL) 
			{
				//If an internal graph exists for this segment ... set visible the next segment in the graph.
				if (crtSegment->internalSegGraph != NULL) 
				{
					crtSegment->internalGraphSelectionIndex += 1;
					if (crtSegment->internalGraphSelectionIndex > crtSegment->internalSegGraph->SurfaceSegmentsCount()-1) crtSegment->internalGraphSelectionIndex = 0;
					std::cout << "[Main] Showing Next Primitive id=" << crtSegment->internalGraphSelectionIndex << " in Segment with id=" << currentlyViewedSegment << std::endl;
					((KDTreePointCloud*)(gp->GetShape()))->SetVisibleNextPrimitiveInSegment(currentlyViewedSegment, crtSegment->internalGraphSelectionIndex);
				}
				else
				{
					std::cout << "[Main] Segment " << currentlyViewedSegment << " does not have an internal graph " << std::endl;
				}
			}
			accuReset = true;
		}

		//---Write The structure graph to file.
		/*if (!processing_visibility_request && !processing_pointcloud_switch_request && key.GetKeyState('Z') == KeyboardStates::clicked)
		{
			std::cout << "[Main] Drawing structure graph to file" << std::endl;
			pcs->m_sceneToSolve->DrawSegmentsGraph(pcs->m_sceneToSolve->m_segmentsGraph, pcs->m_sceneToSolve->m_pointCloud->GetName());
			pcs->m_sceneToSolve->DrawSceneGraph(pcs->m_sceneToSolve->m_structureGraph, pcs->m_sceneToSolve->m_pointCloud->GetName(), 1.2f);
		}*/

		//---Show points which have not been fitted by RANSAC
		if (!processing_visibility_request && !processing_pointcloud_switch_request && key.GetKeyState('N') == KeyboardStates::clicked)
		{										
			SegmentsGraph::SurfaceSegment * crtSegment = pcs->m_sceneToSolve->GetSegmentsGraph()->SurfaceSegmentWithId(currentlyViewedSegment);
			//If an internal graph exists for this segment ... set visible the next segment in the graph.
			if (crtSegment->internalSegGraph != NULL) 
			{							
				std::cout << "[Main] Showing Non Fitted Points in Segment with id " << currentlyViewedSegment << std::endl;
				((KDTreePointCloud*)(gp->GetShape()))->SetVisibleNonFittedPointsInSegment(currentlyViewedSegment);
			}
			else
			{
				std::cout << "[Main] Segment " << currentlyViewedSegment << " does not have an internal graph " << std::endl;
			}
			accuReset = true;
		}

		//---Switch pointclouds 
		if (!processing_visibility_request && !processing_pointcloud_switch_request && key.GetKeyState('P') == KeyboardStates::clicked)
		{							
			processing_pointcloud_switch_request = true;
			std::cout << "[Main] Input Pointcloud ID ... " << std::endl;
			new_pointcloud_id.clear();
			new_pointcloud_view.clear();
		}

		if (processing_pointcloud_switch_request && key.GetKeyState('P') == KeyboardStates::clicked)
		{	
			processing_pointcloud_switch_request = false;
			processing_pointcloud_switch_request_view = false;
			accuReset = true;
			try
			{
				processing_visibility_segmentid = false;
				int pointcloud_id = boost::lexical_cast<int, std::string>(new_pointcloud_id);
				std::cout << "[Main] Loading Pointcloud with id " << new_pointcloud_id << std::endl;
				if (pointcloud_id != 0)
				{
					if (pcs->m_pPreloadedScenes->find(pointcloud_id) == pcs->m_pPreloadedScenes->end() || new_pointcloud_view.length() > 0)
					{
						bool loaded = false;
						int pointcloud_view = 0;
						if (new_pointcloud_view.length() > 0)
						{
							pointcloud_view = boost::lexical_cast<int, std::string>(new_pointcloud_view);
							pointcloud_id += pointcloud_view * 100;
						}
						PointCloudSegmentation * pcs_loaded = new PointCloudSegmentation(pcs->m_pEnvironment, "Output/PCS/"+new_pointcloud_id+"/", pointcloud_id, loaded);
						if (!loaded)
						{
							//Loading Failed
							std::cout << "[Main] Pointcloud with id " << new_pointcloud_id << " FAILED." << std::endl;
						}
						else
						{
							std::cout << "[Main] Pointcloud with id " << new_pointcloud_id << " LOADED." << std::endl;
							pcs->m_sceneToSolve = pcs_loaded;			
							gp->SetShape(pcs_loaded->m_pointCloud);
							pcs_loaded->m_pointCloud->SetEnvironment(pcs->m_pEnvironment);
							(*(pcs->m_pPreloadedScenes))[pointcloud_id] = pcs_loaded;
							((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->ClearAllGrids();
							((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->ClearAllOBBs();
							std::string trunk = boost::lexical_cast<std::string, int>(((KDTreePointCloud*)(gp->GetShape()))->GetPCSId());
							if (camera_parameters->find(new_pointcloud_id) != camera_parameters->end())
							{
								std::cout << "[Setting Camera Parameters] - Trunk " << new_pointcloud_id << " found. Resetting Camera Position" << std::endl;
								std::pair<std::vector<Vector3>, std::vector<float>> cam_parameters = camera_parameters->find(new_pointcloud_id)->second;
								pCamera->MoveTo(cam_parameters.first[0]);
								pCamera->Look(cam_parameters.first[1], cam_parameters.first[2]);
								theta_x = cam_parameters.second[0];
								theta_y = cam_parameters.second[1];
								theta_z = cam_parameters.second[2];
								gp->WorldTransform.SetRotation(Matrix3x3::CreateRotation(Vector3::UnitXPos, theta_x) * 
																Matrix3x3::CreateRotation(Vector3::UnitYPos, theta_y) * 
																Matrix3x3::CreateRotation(Vector3::UnitZPos, theta_z));
								accuReset=true;
							}
						}
					}
					else
					{
						std::cout << "[Main] Loading Pointcloud #" << new_pointcloud_id << " from cache." << std::endl;
						gp->SetShape(pcs->m_pPreloadedScenes->at(pointcloud_id)->m_pointCloud);
					}
				}
				else
				{
					std::cout << "[Main] Resetting Scene to Original." << std::endl;
					gp->SetShape(pcs->m_sceneToSolve->m_pointCloud);
				}							
			} catch (boost::bad_lexical_cast) {				
				std::cout << "[EXCEPTION THROWN] Boost::Bad_lexical_cast" << std::endl;
			}
		}

		if (processing_pointcloud_switch_request && key.GetKeyState('V') == KeyboardStates::clicked)
		{										
			std::cout << "[Main] Input Pointcloud ID View number for... " << new_pointcloud_id << std::endl;			
			new_pointcloud_view.clear();
			processing_pointcloud_switch_request_view = true;
		}

		if (processing_pointcloud_switch_request)
		{
			if (!processing_pointcloud_switch_request_view)
			{
				if (key.GetKeyState('0') == KeyboardStates::clicked) new_pointcloud_id.append("0");
				if (key.GetKeyState('1') == KeyboardStates::clicked) new_pointcloud_id.append("1");
				if (key.GetKeyState('2') == KeyboardStates::clicked) new_pointcloud_id.append("2");
				if (key.GetKeyState('3') == KeyboardStates::clicked) new_pointcloud_id.append("3");
				if (key.GetKeyState('4') == KeyboardStates::clicked) new_pointcloud_id.append("4");
				if (key.GetKeyState('5') == KeyboardStates::clicked) new_pointcloud_id.append("5");
				if (key.GetKeyState('6') == KeyboardStates::clicked) new_pointcloud_id.append("6");
				if (key.GetKeyState('7') == KeyboardStates::clicked) new_pointcloud_id.append("7");
				if (key.GetKeyState('8') == KeyboardStates::clicked) new_pointcloud_id.append("8");
				if (key.GetKeyState('9') == KeyboardStates::clicked) new_pointcloud_id.append("9");
			}
			else
			{
				if (key.GetKeyState('0') == KeyboardStates::clicked) new_pointcloud_view.append("0");
				if (key.GetKeyState('1') == KeyboardStates::clicked) new_pointcloud_view.append("1");
				if (key.GetKeyState('2') == KeyboardStates::clicked) new_pointcloud_view.append("2");
				if (key.GetKeyState('3') == KeyboardStates::clicked) new_pointcloud_view.append("3");
				if (key.GetKeyState('4') == KeyboardStates::clicked) new_pointcloud_view.append("4");
				if (key.GetKeyState('5') == KeyboardStates::clicked) new_pointcloud_view.append("5");
				if (key.GetKeyState('6') == KeyboardStates::clicked) new_pointcloud_view.append("6");
				if (key.GetKeyState('7') == KeyboardStates::clicked) new_pointcloud_view.append("7");
				if (key.GetKeyState('8') == KeyboardStates::clicked) new_pointcloud_view.append("8");
				if (key.GetKeyState('9') == KeyboardStates::clicked) new_pointcloud_view.append("9");
			}
		}
	}		
		
}

void IlluminaPRT(bool p_bVerbose, int p_nVerboseFrequency, 
	int p_nIterations, int p_nThreads, int p_nFPS, 
	int p_nJobs, int p_nSize, int p_nFlags, int p_nTraining, float p_fCameraSpeed,
	std::string p_strScript)
{
	//----------------------------------------------------------------------------------------------
	// Parse flags
	//----------------------------------------------------------------------------------------------
	bool bToneMappingEnabled			= (p_nFlags & 0x01), 
		bDiscontinuityBufferEnabled		= (p_nFlags & 0x02),
		bFrameReconstructionEnabled		= (p_nFlags & 0x04),
		bAccumulationBufferEnabled		= (p_nFlags & 0x08),
		bOutputToNullDeviceEnabled		= (p_nFlags & 0x10);

	if (p_bVerbose)
	{
		std::cout << "-- Tone Mapping [" << bToneMappingEnabled << "]" << std::endl;
		std::cout << "-- Discontinuity Buffer [" << bDiscontinuityBufferEnabled << "]" << std::endl;
		std::cout << "-- Frame Reconstruction [" << bFrameReconstructionEnabled << "]" << std::endl;
		std::cout << "-- Accumulation Buffer [" << bAccumulationBufferEnabled << "]" << std::endl;
		std::cout << "-- Null Device Output [" << bOutputToNullDeviceEnabled << "]" << std::endl;
	}

	//----------------------------------------------------------------------------------------------
	// Illumina sandbox environment 
	//----------------------------------------------------------------------------------------------
	SandboxEnvironment sandbox;	
	sandbox.Initialise(p_bVerbose);
	if (!sandbox.LoadScene(p_strScript, p_bVerbose))
	{
		sandbox.Shutdown(p_bVerbose);
		return;
	}	
	//----------------------------------------------------------------------------------------------
	// Alias required components
	//----------------------------------------------------------------------------------------------
	IIntegrator *pIntegrator = sandbox.GetEnvironment()->GetIntegrator();
	IRenderer *pRenderer = sandbox.GetEnvironment()->GetRenderer();
	ICamera *pCamera = sandbox.GetEnvironment()->GetCamera();
	ISpace *pSpace = sandbox.GetEnvironment()->GetSpace();

	Environment *pEnvironment = sandbox.GetEnvironment();
	EngineKernel *pEngineKernel = sandbox.GetEngineKernel();
	
	PointCloudSolver *pcs = new PointCloudSolver(pEnvironment);	
	// Open output device
	pRenderer->GetDevice()->Open();

	// Initialisation complete
	pEnvironment->GetLogger()->Write("Initialisation complete. Rendering in progress...", LL_Info);	

	//----------------------------------------------------------------------------------------------
	// Post processing setup
	//----------------------------------------------------------------------------------------------
	RadianceBuffer *pRadianceBuffer = new RadianceBuffer(
		pRenderer->GetDevice()->GetWidth(), pRenderer->GetDevice()->GetHeight()),
		*pRadianceAccumulationBuffer = new RadianceBuffer(
		pRenderer->GetDevice()->GetWidth(), pRenderer->GetDevice()->GetHeight());

	IPostProcess *pDiscontinuityBuffer = pEngineKernel->GetPostProcessManager()->CreateInstance("Discontinuity", "DiscontinuityBuffer", "");
	IPostProcess *pAutoTone = pEngineKernel->GetPostProcessManager()->CreateInstance("AutoTone", "AutoTone", "");
	IPostProcess *pDragoTone = pEngineKernel->GetPostProcessManager()->CreateInstance("GlobalTone", "GlobalTone", "");
	IPostProcess *pReconstructionBuffer = pEngineKernel->GetPostProcessManager()->CreateInstance("Reconstruction", "ReconstructionBuffer", "");
	
	AccumulationBuffer *pAccumulationBuffer = (AccumulationBuffer*)pEngineKernel->GetPostProcessManager()->CreateInstance("Accumulation", "AccumulationBuffer", "");
	pAccumulationBuffer->SetAccumulationBuffer(pRadianceAccumulationBuffer);
	pAccumulationBuffer->Reset();

	//----------------------------------------------------------------------------------------------
	// Rendering threads setup
	//----------------------------------------------------------------------------------------------
	RenderThread::RenderThreadState renderThreadState(pRenderer, pRadianceBuffer, p_nSize, p_nSize, p_nThreads + 1);
	renderThreadState.Reset();
	renderThreadState.Run();

	renderThreadState.GenerateTilePackets(p_nJobs);

	for (int threadIdx = 0; threadIdx < p_nThreads; threadIdx++)
		boost::thread runThread(boost::bind(&RenderThread::Render, &renderThreadState));

	//----------------------------------------------------------------------------------------------
	// Rendering budget setup
	//----------------------------------------------------------------------------------------------
	if (p_nFPS != 0)
	{
		int tileCount = (pRenderer->GetDevice()->GetWidth() / p_nSize) * 
			(pRenderer->GetDevice()->GetHeight() / p_nSize);

		float perTileBudget = 1.f / (p_nFPS * (tileCount / p_nThreads));
		pRenderer->SetRenderBudget(perTileBudget);
	}
	else
		pRenderer->SetRenderBudget(10000);

	//----------------------------------------------------------------------------------------------
	// Initialise timing
	//----------------------------------------------------------------------------------------------
	double start, elapsed = 0, eventStart, eventComplete;
	
	double fTotalFramesPerSecond = 0,
		fFrameTime = 0, fTotalTime = 0,
		fFrameIntegratorTime = 0, fTotalIntegratorTime = 0,
		fFrameSpaceTime = 0, fTotalSpaceTime = 0,
		fFrameRadianceTime = 0, fTotalRadianceTime = 0,
		fFramePostProcessingTime = 0, fTotalPostProcessingTime = 0,
		fFrameCommitTime = 0, fTotalCommitTime = 0;

	int nFramesProcessed = 0;

	//----------------------------------------------------------------------------------------------
	// Render loop
	//----------------------------------------------------------------------------------------------
	//float alpha = Maths::Pi;
	//Matrix3x3 rotation;
	//Vector3 observer = pCamera->GetObserver	
	boost::thread inputThread(boost::bind(ReadFromConsole, pEnvironment, pEngineKernel, pcs, p_fCameraSpeed));	
	//InputHandler input_handler = InputHandler(pEnvironment, pEngineKernel, pcs, p_fCameraSpeed);

	for (int nFrame = 0; nFrame < p_nIterations; ++nFrame)
	{
		// Animate scene 
		//alpha += Maths::PiTwo / 180.f;
		
		//rotation.MakeRotation(Vector3::UnitYPos, alpha);

		////((GeometricPrimitive*)pSpace->PrimitiveList[0])->WorldTransform.SetScaling(Vector3::Ones * 20.0f);
		//// ((GeometricPrimitive*)pSpace->PrimitiveList[0])->WorldTransform.SetRotation(rotation);
		
		//pCamera->Move(pCamera->GetFrame().GetW() * 0.01f);
		////pCamera->MoveTo(Vector3(Maths::Cos(alpha) * lookFrom.X, lookFrom.Y, Maths::Sin(alpha) * lookFrom.Z));
		////pCamera->LookAt(lookAt);

		//Vector3 observer_ = observer;
		//observer_.Z += Maths::Cos(alpha) * 4.f;
		//observer_.X += Maths::Sin(alpha) * 2.f;
		//pCamera->MoveTo(observer_);
								
		//----------------------------------------------------------------------------------------------
		// Integrator preparation
		//----------------------------------------------------------------------------------------------
		eventStart = start = Platform::GetTime();

		// Prepare integrator
		pIntegrator->Prepare(pEnvironment->GetScene());

		// Time integrator event
		eventComplete = Platform::GetTime();
		fFrameIntegratorTime = Platform::ToSeconds(eventComplete - eventStart);
		fTotalIntegratorTime += fFrameIntegratorTime;

		//----------------------------------------------------------------------------------------------
		// Space update 
		//----------------------------------------------------------------------------------------------
		eventStart = Platform::GetTime();

		// Update space
		pSpace->Update();

		// Time space update event
		eventComplete = Platform::GetTime();
		fFrameSpaceTime = Platform::ToSeconds(eventComplete - eventStart);
		fTotalSpaceTime += fFrameSpaceTime;

		//----------------------------------------------------------------------------------------------
		// Radiance computation 
		//----------------------------------------------------------------------------------------------
		eventStart = Platform::GetTime();

		// Render phase
		renderThreadState.Reset();
		renderThreadState.Wait();
		renderThreadState.Wait();		

		// Time radiance computation event
		eventComplete = Platform::GetTime();
		fFrameRadianceTime = Platform::ToSeconds(eventComplete - eventStart);
		fTotalRadianceTime += fFrameRadianceTime;

		//----------------------------------------------------------------------------------------------
		// Post processing 
		//----------------------------------------------------------------------------------------------
		eventStart = Platform::GetTime();

		// Reconstruction
		if (bFrameReconstructionEnabled)
			pReconstructionBuffer->Apply(pRadianceBuffer, pRadianceBuffer);
		
		// Discontinuity
		if (bDiscontinuityBufferEnabled)
			pDiscontinuityBuffer->Apply(pRadianceBuffer, pRadianceBuffer);

		// Accumulation		
		if (bAccumulationBufferEnabled)
		{			
			if (accuReset) 
			{
				pAccumulationBuffer->Reset();
				accuReset=false;
			}			
			else
			{
				pAccumulationBuffer->Apply(pRadianceBuffer, pRadianceBuffer);
			}
		}
		else
			pEnvironment->GetScene()->GetSampler()->Reset();		

		// Tonemapping
		if (bToneMappingEnabled)
			pAutoTone->Apply(pRadianceBuffer, pRadianceBuffer);

		// Time radiance computation event
		eventComplete = Platform::GetTime();
		fFramePostProcessingTime = Platform::ToSeconds(eventComplete - eventStart);
		fTotalPostProcessingTime += fFramePostProcessingTime;

		//----------------------------------------------------------------------------------------------
		// Output 
		//----------------------------------------------------------------------------------------------
		eventStart = Platform::GetTime();
				
		// Commit frame
		if (!bOutputToNullDeviceEnabled)
			pRenderer->Commit(pRadianceBuffer);

		// Time radiance computation event
		eventComplete = Platform::GetTime();
		fFrameCommitTime = Platform::ToSeconds(eventComplete - eventStart);
		fTotalCommitTime += fFrameCommitTime;

		//----------------------------------------------------------------------------------------------
		// Frame time computations 
		//----------------------------------------------------------------------------------------------
		// Compute frame time
		fFrameTime = elapsed = Platform::ToSeconds(Platform::GetTime() - start);
		fTotalTime += fFrameTime;

		// Compute total fps
		fTotalFramesPerSecond += (float)(1.f/fFrameTime);

		// Increment number of computed frames
		nFramesProcessed++;		
		
		if (pcs->history_viewing_enabled)
		{		
			if (pcs->m_sceneSolution->history->GetNextCrtViewActiveHistoryLine() < pcs->m_sceneSolution->history->GetActiveHistoryLine()->size())			
			{
				pcs->m_sceneSolution->crt_history_element = pcs->m_sceneSolution->history->GetActiveHistoryLine()->at(pcs->m_sceneSolution->history->GetNextCrtViewActiveHistoryLine());
				FeatureDescStructureGraph::VoxelGrid * crtVoxel = pcs->m_sceneSolution->crt_history_element.vg;

				if (pcs->m_sceneSolution->history->GetNextCrtViewActiveHistoryLine() == 0)
				{
					std::cout << "One time insertion of SegmentGrid in Space" << std::endl;
					pcs->m_sceneSolution->history->grid_gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("GridMaterial"));
					((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddSegmentGrid("history_"+boost::lexical_cast<std::string, int>(nFramesProcessed), pcs->m_sceneSolution->history->grid_gp);

					std::cout << "One time insertion of Model in Space" << std::endl;
					pcs->m_sceneSolution->history_model_in_view = pcs->m_sceneSolution->crt_history_element.model_id;
					pcs->m_sceneSolution->history->model_gp->SetMaterial(pcs->m_pEnvironment->GetEngineKernel()->GetMaterialManager()->RequestInstance("LitMaterial"));
					std::string name = "obj_"+boost::lexical_cast<std::string, int>(pcs->m_sceneSolution->crt_history_element.segs.front());
					pcs->m_sceneSolution->history->model_gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
					std::pair<bool, KDTreeMesh*> shape = pcs->m_trainedModelsDB->GetShape(boost::lexical_cast<std::string, int>(pcs->m_sceneSolution->crt_history_element.model_id));
					pcs->m_sceneSolution->history->model_gp->SetShape(shape.second);
					((BasicSpacePCS*)pcs->m_pEnvironment->GetSpace())->AddModel(name, pcs->m_sceneSolution->history->model_gp);
					if (pcs->m_sceneSolution->model_original_positions->find(pcs->m_sceneSolution->crt_history_element.model_id) == pcs->m_sceneSolution->model_original_positions->end())
					{
						(*pcs->m_sceneSolution->model_original_positions)[pcs->m_sceneSolution->crt_history_element.model_id] = new std::vector<Vector3>();
						for (int i=0; i<shape.second->VertexList.Size(); i++)
						{
							(*pcs->m_sceneSolution->model_original_positions)[pcs->m_sceneSolution->crt_history_element.model_id]->push_back(Vector3(shape.second->VertexList.At(i).Position));
						}
					}
				}

				//show the next grid				
				crtVoxel->SetActiveCells(*(pcs->m_sceneSolution->history->grid_spheres));
				pcs->m_sceneSolution->history->sphereMesh->Clear();
				
				for (auto s : *(pcs->m_sceneSolution->history->grid_spheres))
				{				
					pcs->m_sceneSolution->history->sphereMesh->AddSphere(s.second, s.first.second, s.first.first);
				}
								
				pcs->m_sceneSolution->history->sphereMesh->ComputeBoundingVolume();
				pcs->m_sceneSolution->history->sphereMesh->Compile();
				pcs->m_sceneSolution->history->grid_gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;

				//and the transformed model
				//first reset to original positions.
				//check if model has been changed first.
				if (pcs->m_sceneSolution->history_model_in_view != pcs->m_sceneSolution->crt_history_element.model_id)
				{
					std::cout << "Swapping Models!!!" << std::endl;
					std::pair<bool, KDTreeMesh*> shape = pcs->m_trainedModelsDB->GetShape(boost::lexical_cast<std::string, int>(pcs->m_sceneSolution->crt_history_element.model_id));					
					pcs->m_sceneSolution->history->model_gp->SetShape(shape.second);					
					if (pcs->m_sceneSolution->model_original_positions->find(pcs->m_sceneSolution->crt_history_element.model_id) == pcs->m_sceneSolution->model_original_positions->end())
					{
						(*pcs->m_sceneSolution->model_original_positions)[pcs->m_sceneSolution->crt_history_element.model_id] = new std::vector<Vector3>();
						for (int i=0; i<shape.second->VertexList.Size(); i++)
						{
							(*pcs->m_sceneSolution->model_original_positions)[pcs->m_sceneSolution->crt_history_element.model_id]->push_back(Vector3(shape.second->VertexList.At(i).Position));
						}
					}
					pcs->m_sceneSolution->history_model_in_view = pcs->m_sceneSolution->crt_history_element.model_id;
				}
				for (int i = 0; i<((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->VertexList.Size(); i++) 
				{
					((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->VertexList[i].Position.X = (*pcs->m_sceneSolution->model_original_positions)[pcs->m_sceneSolution->crt_history_element.model_id]->at(i).X;
					((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->VertexList[i].Position.Y = (*pcs->m_sceneSolution->model_original_positions)[pcs->m_sceneSolution->crt_history_element.model_id]->at(i).Y;
					((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->VertexList[i].Position.Z = (*pcs->m_sceneSolution->model_original_positions)[pcs->m_sceneSolution->crt_history_element.model_id]->at(i).Z; 
				}			
				pcs->m_sceneSolution->crt_history_element.vg->TransformModelVertices(((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->VertexList, pcs->m_sceneSolution->crt_history_element.tgrid_origin, pcs->m_sceneSolution->crt_history_element.grid_idx, 
																						pcs->m_sceneSolution->crt_history_element.u, pcs->m_sceneSolution->crt_history_element.v, pcs->m_sceneSolution->crt_history_element.w);
				for (int i = 0; i<((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->TriangleList.Size(); i++) 
				{ 
					((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->TriangleList[i].ComputeEdges(); 
				}	
				((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->ComputeBoundingVolume();
				((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->Compile();
				pcs->m_sceneSolution->history->model_gp->WorldTransform = ((GeometricPrimitive*)(pcs->m_pEnvironment->GetSpace()->PrimitiveList.Front()))->WorldTransform;
				//Updates to model done.

				if (pcs->history_segments_in_view.size() < pcs->m_sceneSolution->crt_history_element.segs.size())
				{
					pcs->history_segments_in_view.push_back(pcs->m_sceneSolution->crt_history_element.segs.at(pcs->history_segments_in_view.size()));
					pcs->m_sceneToSolve->m_pointCloud->SetVisiblePointsWithSegmentId(pcs->history_segments_in_view.at(pcs->history_segments_in_view.size()-1));
					std::cout << "\n***********\nNow Considering Segments :: ";
					for (auto ii : pcs->history_segments_in_view) std::cout << ii << " "; 
					std::cout << "\n***********" << std::endl;
				}

				if (pcs->history_segments_in_view.size() > pcs->m_sceneSolution->crt_history_element.segs.size())
				{
					for (auto ii : pcs->history_segments_in_view) pcs->m_sceneToSolve->m_pointCloud->SetInvisiblePointsWithSegmentId(ii); 
					pcs->history_segments_in_view.clear();
					pcs->history_segments_in_view.push_back(pcs->m_sceneSolution->crt_history_element.segs.at(0));					
					pcs->m_sceneToSolve->m_pointCloud->SetVisiblePointsWithSegmentId(pcs->history_segments_in_view.at(pcs->history_segments_in_view.size()-1));
					std::cout << "\n***********RESET*********\nNow Considering Segments :: ";
					for (auto ii : pcs->history_segments_in_view) std::cout << ii << " "; 
					std::cout << "\n***********RESET*********" << std::endl;
				}
				
				std::cout << "-H@" << pcs->m_sceneSolution->history->GetNextCrtViewActiveHistoryLine(); // << " OriginXYZ: " << crtVoxel->o_model_xyz.ToString() << " KDTreeSize=" << pcs->m_sceneSolution->history->sphereMesh->SphereList.Size();
				std::cout << " @rot " << crtVoxel->rotation << " Cov:"<< pcs->m_sceneSolution->crt_history_element.coverage << 
								" IncBias:" << pcs->m_sceneSolution->crt_history_element.inclusion_bias <<  
								" AnchorMatchBias:" << pcs->m_sceneSolution->crt_history_element.anchor_match_bias << 
								" NonAnchorMatchBias:" << pcs->m_sceneSolution->crt_history_element.non_anchor_match_bias << 
								" AnchorMatches:" << pcs->m_sceneSolution->crt_history_element.pos_match_count << 
								" NonAnchorMatches:" << pcs->m_sceneSolution->crt_history_element.neg_match_count << std::endl;
				
				/*std::cout << "0-History@" << pcs->m_sceneSolution->history->GetNextCrtViewActiveHistoryLine(); 				
				std::cout << " KDTreeSize=" << pcs->m_sceneSolution->history->m_vg_history_0->at(pcs->m_sceneSolution->history->GetNextCrtViewActiveHistoryLine()).vg->m_voxels->size();
				std::cout << " @rot " << pcs->m_sceneSolution->history->m_vg_history_0->at(pcs->m_sceneSolution->history->GetNextCrtViewActiveHistoryLine()).vg->rotation;
				std::cout << " Cov:"<< pcs->m_sceneSolution->history->m_vg_history_0->at(pcs->m_sceneSolution->history->GetNextCrtViewActiveHistoryLine()).coverage << std::endl;

				std::cout << "4-History@" << pcs->m_sceneSolution->history->GetNextCrtViewActiveHistoryLine(); 
				std::cout << " KDTreeSize=" << pcs->m_sceneSolution->history->m_vg_history_4->at(pcs->m_sceneSolution->history->GetNextCrtViewActiveHistoryLine()).vg->m_voxels->size();
				std::cout << " @rot " << pcs->m_sceneSolution->history->m_vg_history_4->at(pcs->m_sceneSolution->history->GetNextCrtViewActiveHistoryLine()).vg->rotation;
				std::cout << " Cov:"<< pcs->m_sceneSolution->history->m_vg_history_4->at(pcs->m_sceneSolution->history->GetNextCrtViewActiveHistoryLine()).coverage << std::endl;
				*/
				
				pcs->m_sceneSolution->history->UpdateIndexForBestCoverage(pcs->m_sceneSolution->crt_history_element.coverage);
				pcs->m_sceneSolution->history->BulkIncCrtViewActiveHistoryLine_Coverage();

				//pcs->m_sceneSolution->history->UpdateIndexForBestNonAnchorMatches(pcs->m_sceneSolution->crt_history_element.neg_match_count);								
				//pcs->m_sceneSolution->history->BulkIncCrtViewActiveHistoryLine_NonAnchorMatches();

				//pcs->m_sceneSolution->history->UpdateIndexForBestAnchorMatches(pcs->m_sceneSolution->crt_history_element.pos_match_count);
				//pcs->m_sceneSolution->history->BulkIncCrtViewActiveHistoryLine_AnchorMatches();

				//pcs->m_sceneSolution->history->UpdateIndexForBestAnchorPlusNonAnchorMatches(pcs->m_sceneSolution->crt_history_element.pos_match_count + pcs->m_sceneSolution->crt_history_element.neg_match_count);
				//pcs->m_sceneSolution->history->BulkIncCrtViewActiveHistoryLine_Non_Plus_AnchorMatches();

				//pcs->m_sceneSolution->history->AngleIncCrtViewActiveHistoryLine();				
				//pcs->m_sceneSolution->history->IncCrtViewActiveHistoryLine(1);
				pAccumulationBuffer->Reset();
			}
			else
			{
				//Scale Model in Scene - Apply Scale to vertices.				
				for (int i = 0; i<((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->VertexList.Size(); i++) 
				{
					((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->VertexList[i].Position.X = (*pcs->m_sceneSolution->model_original_positions)[pcs->m_sceneSolution->crt_history_element.model_id]->at(i).X;
					((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->VertexList[i].Position.Y = (*pcs->m_sceneSolution->model_original_positions)[pcs->m_sceneSolution->crt_history_element.model_id]->at(i).Y;
					((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->VertexList[i].Position.Z = (*pcs->m_sceneSolution->model_original_positions)[pcs->m_sceneSolution->crt_history_element.model_id]->at(i).Z; 
				}

				pcs->m_sceneSolution->crt_history_element.vg->ScaleToGridModelVertices(((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->VertexList, pcs->m_sceneSolution->crt_history_element.tgrid_origin, pcs->m_sceneSolution->crt_history_element.grid_idx, 
																						pcs->m_sceneSolution->crt_history_element.u, pcs->m_sceneSolution->crt_history_element.v, pcs->m_sceneSolution->crt_history_element.w);
												


				for (int i = 0; i<((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->TriangleList.Size(); i++) 
				{ 
					((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->TriangleList[i].ComputeEdges();
				}	
				
				((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->ComputeBoundingVolume();
				((KDTreeMesh*)pcs->m_sceneSolution->history->model_gp->GetShape())->Compile();

				if (pcs->m_sceneSolution->histories->MoreHistory())
				{
					pcs->m_sceneSolution->history = pcs->m_sceneSolution->histories->GetNextHistory();
					pcs->m_sceneSolution->history->SetActiveHistoryLine(8);					
					pcs->m_sceneSolution->histories->IncHistoriesIndex();
					pcs->history_segments_in_view.clear();					
					std::cout << "Done - Switching to next history - Best Coverage of " << pcs->m_sceneSolution->history->best_coverage_score << std::endl;
				}
				else
				{
					pcs->history_viewing_enabled=false;
					pcs->history_segments_in_view.clear();				
					std::cout << "Done - Switching off animation of history - Showing History with Best Coverage of " << pcs->m_sceneSolution->history->best_coverage_score << " idx:" << pcs->m_sceneSolution->history->grid_index_with_best_coverage << std::endl;
				}
			}
		}

		//accuReset = input_handler.HandleInput();

		//if (accuReset) { std::cout << "AccuReset == true" << std::endl;}

		//----------------------------------------------------------------------------------------------
		// Verbose output 
		//----------------------------------------------------------------------------------------------
		if (p_bVerbose && nFramesProcessed % p_nVerboseFrequency == 0)
		{
			std::cout << std::endl << "-- Frame " << nFrame << std::endl;
			std::cout << "--- Integrator Preparation Time : [" << fFrameIntegratorTime << "s]" << std::endl;
			std::cout << "--- Space Update Time : [" << fFrameSpaceTime << "s]" << std::endl;
			std::cout << "--- Radiance Computation Time : [" << fFrameRadianceTime << "s]" << std::endl;
			std::cout << "--- Post-processing Time : [" << fFramePostProcessingTime << "s]" << std::endl;
			std::cout << "--- Output Time : [" << fFrameCommitTime << "s]" << std::endl;
			std::cout << "--- Frame Render Time : [" << fFrameTime << "s]" << std::endl;
			std::cout << "--- Frames per second : [" << fTotalFramesPerSecond / nFramesProcessed << "]" << std::endl;
		}

	}

	//----------------------------------------------------------------------------------------------
	// Stop rendering threads
	//----------------------------------------------------------------------------------------------
	renderThreadState.Stop();

	//----------------------------------------------------------------------------------------------
	// Output averaged timings
	//----------------------------------------------------------------------------------------------
	if (p_bVerbose)
	{
		std::cout << std::endl << "-- Average timings over [" << nFramesProcessed << "] frames" << std::endl;
		std::cout << "--- Integrator Preparation Time : [" << fTotalIntegratorTime / nFramesProcessed << "s]" << std::endl;
		std::cout << "--- Space Update Time : [" << fTotalSpaceTime / nFramesProcessed << "s]" << std::endl;
		std::cout << "--- Radiance Computation Time : [" << fTotalRadianceTime / nFramesProcessed << "s]" << std::endl;
		std::cout << "--- Post-processing Time : [" << fTotalPostProcessingTime / nFramesProcessed << "s]" << std::endl;
		std::cout << "--- Output Time : [" << fTotalCommitTime / nFramesProcessed << "s]" << std::endl;
		std::cout << "--- Frame Render Time : [" << fTotalTime / nFramesProcessed << "s]" << std::endl;
		std::cout << "--- Frames per second : [" << fTotalFramesPerSecond / nFramesProcessed << "]" << std::endl << std::endl;
	}

	// Output per thread statistics
	std::ofstream threadTimings;
	threadTimings.open("threadTimings.txt", std::ios::binary);
	threadTimings << renderThreadState.GetStatistics() << std::endl;
	threadTimings.close();

	//----------------------------------------------------------------------------------------------
	// Shutdown system
	//----------------------------------------------------------------------------------------------
	
	// Close output device
	pRenderer->GetDevice()->Close();

	// Shutdown renderer and integrator
	pRenderer->Shutdown();
	pIntegrator->Shutdown();

	// Shutdown sandbox
	sandbox.Shutdown(p_bVerbose);

	//----------------------------------------------------------------------------------------------
	if (p_bVerbose)
	{
		pEnvironment->GetLogger()->Write("Complete :: Press enter to continue", LL_Critical); std::getchar();
	}
}
//----------------------------------------------------------------------------------------------

int main(int argc, char** argv)
{
	std::cout << "Illumina Renderer : Version " << Illumina::Core::Major << "." << Illumina::Core::Minor << "." << Illumina::Core::Build << " http://www.illuminaprt.codeplex.com " << std::endl;
	std::cout << "Copyright (C) 2010-2013 Keith Bugeja" << std::endl << std::endl;

	// default options
	int nVerboseFrequency = 1,
		nIterations = 1,
		nThreads = 1,
		nSize = 32,
		nJobs = 0x10000,
		nFPS = 5,		
		nFlags = 0xFF;

	float fCameraSpeed;

	bool bVerbose = false;
	bool nTraining = false;
	std::string strScript("default.ilm");

	// Declare the supported options.
	boost::program_options::options_description description("Allowed Settings");

	description.add_options()
		("help", "show this message")
		("verbose", boost::program_options::value<bool>(), "show extended information")
		("statfreq", boost::program_options::value<int>(), "show frame statistics every nth frame (requires verbose)")
		("script", boost::program_options::value<std::string>(), "script file to render")
		("workdir", boost::program_options::value<std::string>(), "working directory")
		("iterations", boost::program_options::value<int>(), "iterations to execute")
		("threads", boost::program_options::value<int>(), "number of rendering threads")
		("tilesize", boost::program_options::value<int>(), "initial length of tile edge")
		("tilejobs", boost::program_options::value<int>(), "number of jobs before tile subdivision")
		("flags", boost::program_options::value<int>(), "rendering flags")
		("fps", boost::program_options::value<int>(), "frame rendering frequency (hint)")
		("cameraspeed", boost::program_options::value<float>(), "camera movement multiplier")
		("training", boost::program_options::value<bool>(), "training mode")
		;

	// Declare variable map
	boost::program_options::variables_map variableMap;

	// Parse command line options
	try 
	{
		boost::program_options::store(boost::program_options::parse_command_line(argc, argv, description), variableMap);
		boost::program_options::notify(variableMap);
	} 
	catch (boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::program_options::unknown_option> > &exception) 
	{
		std::cout << "Unknown option [" << exception.get_option_name() << "] : Please use --help to display help message." << std::endl;
		return 1;
	}
	catch (boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::program_options::invalid_option_value> > &exception) 
	{
		std::cout << "Error parsing input for [" << exception.get_option_name() << "] : Invalid argument value." << std::endl;
		return 1;
	}

	// --help
	if (variableMap.count("help"))
	{
		std::cout << description << std::endl;
		return 1;
	}

	// --verbose
	if (variableMap.count("verbose"))
	{
		bVerbose = variableMap["verbose"].as<bool>();
		std::cout << "Verbose mode [" << (bVerbose ? "ON]" : "OFF]") << std::endl;
	}

	// --statfreq
	if (variableMap.count("statfreq"))
	{
		try {
			nVerboseFrequency = variableMap["statfreq"].as<int>();
		} catch (...) { nVerboseFrequency = 1; } 
		std::cout << "Render statistics output frequency [" << nIterations << "]" << std::endl;
	}

	// --iterations
	if (variableMap.count("iterations"))
	{
		try {
			nIterations = variableMap["iterations"].as<int>();
		} catch (...) { nIterations = 1; } 
		std::cout << "Iterations [" << nIterations << "]" << std::endl;
	}

	// --cameraspeed
	if (variableMap.count("cameraspeed"))
	{
		try {
			fCameraSpeed = variableMap["cameraspeed"].as<float>();
		} catch (...) { fCameraSpeed = 0.1f; } 
		std::cout << "Camera Speed [" << fCameraSpeed << "]" << std::endl;
	}

	// --script
	if (variableMap.count("script"))
	{
		strScript = variableMap["script"].as<std::string>();
		std::cout << "Script [" << strScript << "]" << std::endl;
	}

	// --workdir
	boost::filesystem::path cwdPath;

	if (variableMap.count("workdir"))
	{
		cwdPath = boost::filesystem::path(variableMap["workdir"].as<std::string>());
	}
	else
	{
		// Setting working directory
		boost::filesystem::path scriptPath(strScript);
		cwdPath = boost::filesystem::path(scriptPath.parent_path());
	}

	try {
		boost::filesystem::current_path(cwdPath);
		std::cout << "Working directory [" << cwdPath.string() << "]" << std::endl;;
	} catch (...) { std::cerr << "Error : Unable to set working directory to " << cwdPath.string() << std::endl; }

	// --threads
	if (variableMap.count("threads"))
	{
		try {
			nThreads = variableMap["threads"].as<int>();
		} catch (...) { nThreads = 1; } 
		std::cout << "Threads [" << nThreads << "]" << std::endl;
	}

	// --width
	if (variableMap.count("tilesize"))
	{
		try {
			nSize = variableMap["tilesize"].as<int>();
		} catch (...) { nSize = 16; } 
		std::cout << "Tile size [" << nSize << " x " << nSize << "]" << std::endl;
	}

	// --threads
	if (variableMap.count("tilejobs"))
	{
		try {
			nJobs = variableMap["tilejobs"].as<int>();
		} catch (...) { nJobs = 0x10000; } 
		std::cout << "Jobs before tile subdivision [" << nJobs << "]" << std::endl;
	}

	// --training
	if (variableMap.count("training"))
	{
		try {
			nTraining = variableMap["training"].as<bool>();
		} catch (...) { nTraining = false; } 
		std::cout << "Training Mode [" << nTraining << "]" << std::endl;
	}

	// --budget
	if (variableMap.count("fps"))
	{
		try {
			nFPS = variableMap["fps"].as<int>();
		} catch (...) { nFPS = 0; } 
		std::cout << "FPS [" << nFPS << "]" << std::endl;
	}

	// --flags
	if (variableMap.count("flags"))
	{
		try {
			nFlags = variableMap["flags"].as<int>();
		} catch (...) { nFlags = 0x01 | 0x02 | 0x04 | 0x08; } 
		std::cout << "Flags [" << nFlags << "]" << std::endl;
	}

	// -- start rendering
	IlluminaPRT(bVerbose, nVerboseFrequency, nIterations, nThreads, nFPS, nJobs, nSize, nFlags, nTraining, fCameraSpeed, strScript);

	// Exit
	return 1;
}
